diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs index 4770b8f..eb2ebec 100644 --- a/satrs-example/src/acs/mgm.rs +++ b/satrs-example/src/acs/mgm.rs @@ -1,7 +1,6 @@ -// TODO: Remove this at a later stage. -#![allow(dead_code)] use derive_new::new; -use satrs::hk::HkRequestVariant; +use satrs::hk::{HkRequest, HkRequestVariant}; +use satrs::queue::{GenericSendError, GenericTargetedMessagingError}; use satrs::spacepackets::ecss::hk; use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader}; use satrs::spacepackets::SpHeader; @@ -23,16 +22,23 @@ use serde::{Deserialize, Serialize}; pub trait SpiInterface { type Error; - fn transfer(&mut self, data: &mut [u8]) -> Result<(), Self::Error>; + fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error>; } #[derive(Default)] -pub struct SpiDummyInterface {} +pub struct SpiDummyInterface { + dummy_val_0: i16, + dummy_val_1: i16, + dummy_val_2: i16, +} impl SpiInterface for SpiDummyInterface { type Error = (); - fn transfer(&mut self, _data: &mut [u8]) -> Result<(), Self::Error> { + fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> { + rx[0..2].copy_from_slice(&self.dummy_val_0.to_be_bytes()); + rx[2..4].copy_from_slice(&self.dummy_val_1.to_be_bytes()); + rx[4..6].copy_from_slice(&self.dummy_val_2.to_be_bytes()); Ok(()) } } @@ -64,81 +70,106 @@ pub struct MgmHandler { #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")] mode: ModeAndSubmode, #[new(default)] + tx_buf: [u8; 12], + #[new(default)] + rx_buf: [u8; 12], + #[new(default)] stamp_helper: TimeStampHelper, } impl MgmHandler { pub fn periodic_operation(&mut self) { self.stamp_helper.update_from_now(); - // Handle messages. - // TODO: Put this in loop/seprate function. - match self.composite_request_receiver.try_recv() { - Ok(ref msg) => match &msg.message { - CompositeRequest::Hk(hk_req) => match hk_req.variant { - HkRequestVariant::OneShot => { - self.hk_reply_sender - .send(GenericMessage::new( - msg.requestor_info, - HkReply::new(hk_req.unique_id, HkReplyVariant::Ack), - )) - .expect("failed to send HK reply"); - let mut sp_header = SpHeader::tm_unseg(self.id.apid, 0, 0).unwrap(); - let sec_header = PusTmSecondaryHeader::new( - 3, - hk::Subservice::TmHkPacket as u8, - 0, - 0, - Some(self.stamp_helper.stamp()), - ); - // Let's serialize it as JSON for now.. This is a lot simpler than binary - // serialization. - let mgm_data_serialized = - serde_json::to_vec(&*self.shared_mgm_set.lock().unwrap()).unwrap(); - let hk_tm = PusTmCreator::new( - &mut sp_header, - sec_header, - &mgm_data_serialized, - true, - ); - self.tm_sender - .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) - .expect("failed to send HK TM"); - } - HkRequestVariant::EnablePeriodic => todo!(), - HkRequestVariant::DisablePeriodic => todo!(), - HkRequestVariant::ModifyCollectionInterval(_) => todo!(), - }, - // TODO: This object does not have actions (yet).. Still send back completion failure - // reply. - CompositeRequest::Action(_action_req) => {} - }, + // Handle requests. + self.handle_composite_requests(); + self.handle_mode_requests(); + // Communicate with the device. + let result = self.com_interface.transfer(&self.tx_buf, &mut self.rx_buf); + assert!(result.is_ok()); + // TODO: Convert the raw data back to floats. + } - Err(e) => { - if e != mpsc::TryRecvError::Empty { - log::warn!( - "{}: failed to receive composite request: {:?}", - self.dev_str, - e - ); + pub fn handle_composite_requests(&mut self) { + loop { + match self.composite_request_receiver.try_recv() { + Ok(ref msg) => match &msg.message { + CompositeRequest::Hk(hk_request) => { + self.handle_hk_request(&msg.requestor_info, hk_request) + } + // TODO: This object does not have actions (yet).. Still send back completion failure + // reply. + CompositeRequest::Action(_action_req) => {} + }, + + Err(e) => { + if e != mpsc::TryRecvError::Empty { + log::warn!( + "{}: failed to receive composite request: {:?}", + self.dev_str, + e + ); + } else { + break; + } } } } - // TODO: Put this in loop and only allow one set mode request per cycle? - match self.mode_interface.request_rx.try_recv() { - Ok(msg) => { - let result = self.handle_mode_request(msg); - // TODO: Trigger event? - if result.is_err() { - log::warn!( - "{}: mode request failed with error {:?}", - self.dev_str, - result.err().unwrap() - ); - } + } + + pub fn handle_hk_request(&mut self, requestor_info: &MessageMetadata, hk_request: &HkRequest) { + match hk_request.variant { + HkRequestVariant::OneShot => { + self.hk_reply_sender + .send(GenericMessage::new( + *requestor_info, + HkReply::new(hk_request.unique_id, HkReplyVariant::Ack), + )) + .expect("failed to send HK reply"); + let mut sp_header = SpHeader::tm_unseg(self.id.apid, 0, 0).unwrap(); + let sec_header = PusTmSecondaryHeader::new( + 3, + hk::Subservice::TmHkPacket as u8, + 0, + 0, + Some(self.stamp_helper.stamp()), + ); + // Let's serialize it as JSON for now.. This is a lot simpler than binary + // serialization. + let mgm_data_serialized = + serde_json::to_vec(&*self.shared_mgm_set.lock().unwrap()).unwrap(); + let hk_tm = + PusTmCreator::new(&mut sp_header, sec_header, &mgm_data_serialized, true); + self.tm_sender + .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm)) + .expect("failed to send HK TM"); } - Err(e) => { - if e != mpsc::TryRecvError::Empty { - log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e); + HkRequestVariant::EnablePeriodic => todo!(), + HkRequestVariant::DisablePeriodic => todo!(), + HkRequestVariant::ModifyCollectionInterval(_) => todo!(), + } + } + + pub fn handle_mode_requests(&mut self) { + loop { + // TODO: Only allow one set mode request per cycle? + match self.mode_interface.request_rx.try_recv() { + Ok(msg) => { + let result = self.handle_mode_request(msg); + // TODO: Trigger event? + if result.is_err() { + log::warn!( + "{}: mode request failed with error {:?}", + self.dev_str, + result.err().unwrap() + ); + } + } + Err(e) => { + if e != mpsc::TryRecvError::Empty { + log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e); + } else { + break; + } } } } @@ -167,19 +198,24 @@ impl ModeRequestHandler Ok(()) } - fn announce_mode(&self, _requestor_info: MessageMetadata, _recursive: bool) { + fn announce_mode(&self, _requestor_info: Option, _recursive: bool) { log::info!("{} announcing mode: {:?}", self.dev_str, self.mode); + // TODO: Trigger event. } fn handle_mode_reached( &mut self, requestor: Option, ) -> Result<(), Self::Error> { + self.announce_mode(requestor, false); if let Some(requestor) = requestor { - if requestor.sender_id() == PUS_MODE_SERVICE.raw() { - // self.mode_reply_sender_to_pus.send( - //GenericMessage::new(requestor.request_id, requestor.sender_id, ModeReply::ModeReply(self.mode)) - // )?; + if requestor.sender_id() != PUS_MODE_SERVICE.id() { + log::warn!( + "can not send back mode reply to sender {}", + requestor.sender_id() + ); + } else { + self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode()))?; } } Ok(()) @@ -187,9 +223,19 @@ impl ModeRequestHandler fn send_mode_reply( &self, - _requestor: MessageMetadata, - _reply: ModeReply, + requestor: MessageMetadata, + reply: ModeReply, ) -> Result<(), Self::Error> { + if requestor.sender_id() != PUS_MODE_SERVICE.id() { + log::warn!( + "can not send back mode reply to sender {}", + requestor.sender_id() + ); + } + self.mode_interface + .reply_tx_to_pus + .send(GenericMessage::new(requestor, reply)) + .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?; Ok(()) } diff --git a/satrs/src/mode.rs b/satrs/src/mode.rs index c3a3e21..df2590e 100644 --- a/satrs/src/mode.rs +++ b/satrs/src/mode.rs @@ -190,7 +190,7 @@ pub trait ModeRequestHandler: ModeProvider { mode_and_submode: ModeAndSubmode, ) -> Result<(), Self::Error>; - fn announce_mode(&self, requestor_info: MessageMetadata, recursive: bool); + fn announce_mode(&self, requestor_info: Option, recursive: bool); fn handle_mode_reached( &mut self, @@ -222,11 +222,11 @@ pub trait ModeRequestHandler: ModeProvider { ModeReply::ModeReply(self.mode_and_submode()), ), ModeRequest::AnnounceMode => { - self.announce_mode(request.requestor_info, false); + self.announce_mode(Some(request.requestor_info), false); Ok(()) } ModeRequest::AnnounceModeRecursive => { - self.announce_mode(request.requestor_info, true); + self.announce_mode(Some(request.requestor_info), true); Ok(()) } ModeRequest::ModeInfo(info) => self.handle_mode_info(request.requestor_info, info), diff --git a/satrs/tests/mode_tree.rs b/satrs/tests/mode_tree.rs index 3603e48..17f9836 100644 --- a/satrs/tests/mode_tree.rs +++ b/satrs/tests/mode_tree.rs @@ -79,7 +79,7 @@ impl ModeRequestHandler for TestDevice { Ok(()) } - fn announce_mode(&self, _requestor_info: MessageMetadata, _recursive: bool) { + fn announce_mode(&self, _requestor_info: Option, _recursive: bool) { println!( "{}: announcing mode: {:?}", self.name, self.mode_and_submode @@ -150,9 +150,11 @@ impl TestAssembly { ModeReply::ModeReply(self.mode_and_submode), ) .unwrap(), - ModeRequest::AnnounceMode => self.announce_mode(request.requestor_info, false), + ModeRequest::AnnounceMode => { + self.announce_mode(Some(request.requestor_info), false) + } ModeRequest::AnnounceModeRecursive => { - self.announce_mode(request.requestor_info, true) + self.announce_mode(Some(request.requestor_info), true) } ModeRequest::ModeInfo(_) => todo!(), } @@ -197,7 +199,7 @@ impl ModeRequestHandler for TestAssembly { Ok(()) } - fn announce_mode(&self, requestor_info: MessageMetadata, recursive: bool) { + fn announce_mode(&self, requestor_info: Option, recursive: bool) { println!( "TestAssembly: Announcing mode (recursively: {}): {:?}", recursive, self.mode_and_submode @@ -207,6 +209,7 @@ impl ModeRequestHandler for TestAssembly { if recursive { mode_request = ModeRequest::AnnounceModeRecursive; } + let request_id = requestor_info.map_or(0, |info| info.request_id()); self.mode_node .request_sender_map .0 @@ -214,10 +217,7 @@ impl ModeRequestHandler for TestAssembly { .for_each(|(_, sender)| { sender .send(GenericMessage::new( - MessageMetadata::new( - requestor_info.request_id(), - self.mode_node.local_channel_id_generic(), - ), + MessageMetadata::new(request_id, self.mode_node.local_channel_id_generic()), mode_request, )) .expect("sending mode request failed");