introduce mode commands to python client

satrs-example/satrs-tmtc/pus_tc.py

@@ -22,6 +22,25 @@ def create_cmd_definition_tree() -> CmdTreeNode:
 
     root_node = CmdTreeNode.root_node()
 
+    hk_node = CmdTreeNode("hk", "Housekeeping Node", hide_children_for_print=True)
+    hk_node.add_child(CmdTreeNode("one_shot_hk", "Request One Shot HK set"))
+    hk_node.add_child(
+        CmdTreeNode("enable", "Enable periodic housekeeping data generation")
+    )
+    hk_node.add_child(
+        CmdTreeNode("disable", "Disable periodic housekeeping data generation")
+    )
+
+    mode_node = CmdTreeNode("mode", "Mode Node", hide_children_for_print=True)
+    set_mode_node = CmdTreeNode(
+        "set_mode", "Set Node", hide_children_which_are_leaves=True
+    )
+    set_mode_node.add_child(CmdTreeNode("off", "Set OFF Mode"))
+    set_mode_node.add_child(CmdTreeNode("on", "Set ON Mode"))
+    set_mode_node.add_child(CmdTreeNode("normal", "Set NORMAL Mode"))
+    mode_node.add_child(set_mode_node)
+    mode_node.add_child(CmdTreeNode("read_mode", "Read Mode"))
+
     test_node = CmdTreeNode("test", "Test Node")
     test_node.add_child(CmdTreeNode("ping", "Send PUS ping TC"))
     test_node.add_child(CmdTreeNode("trigger_event", "Send PUS test to trigger event"))
@@ -37,7 +56,9 @@ def create_cmd_definition_tree() -> CmdTreeNode:
 
     acs_node = CmdTreeNode("acs", "ACS Subsystem Node")
     mgm_node = CmdTreeNode("mgms", "MGM devices node")
-    mgm_node.add_child(CmdTreeNode("one_shot_hk", "Request one shot HK"))
+    mgm_node.add_child(mode_node)
+    mgm_node.add_child(hk_node)
+
     acs_node.add_child(mgm_node)
     root_node.add_child(acs_node)
 

satrs-example/src/acs/mgm.rs

@@ -4,7 +4,7 @@ use satrs::queue::{GenericSendError, GenericTargetedMessagingError};
 use satrs::spacepackets::ecss::hk;
 use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader};
 use satrs::spacepackets::SpHeader;
-use satrs_example::TimeStampHelper;
+use satrs_example::{DeviceMode, TimeStampHelper};
 use std::sync::mpsc::{self};
 use std::sync::{Arc, Mutex};
 
@@ -20,6 +20,10 @@ use crate::requests::CompositeRequest;
 
 use serde::{Deserialize, Serialize};
 
+const GAUSS_TO_MICROTESLA_FACTOR: f32 = 100.0;
+// This is the selected resoltion for the STM LIS3MDL device for the 4 Gauss sensitivity setting.
+const FIELD_LSB_PER_GAUSS_4_SENS: f32 = 1.0 / 6842.0;
+
 pub trait SpiInterface {
     type Error;
     fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error>;
@@ -27,9 +31,9 @@ pub trait SpiInterface {
 
 #[derive(Default)]
 pub struct SpiDummyInterface {
-    dummy_val_0: i16,
-    dummy_val_1: i16,
-    dummy_val_2: i16,
+    pub dummy_val_0: i16,
+    pub dummy_val_1: i16,
+    pub dummy_val_2: i16,
 }
 
 impl SpiInterface for SpiDummyInterface {
@@ -45,6 +49,7 @@ impl SpiInterface for SpiDummyInterface {
 
 #[derive(Default, Debug, Copy, Clone, Serialize, Deserialize)]
 pub struct MgmData {
+    pub valid: bool,
     pub x: f32,
     pub y: f32,
     pub z: f32,
@@ -56,9 +61,10 @@ pub struct MpscModeLeafInterface {
     pub reply_tx_to_parent: mpsc::Sender<GenericMessage<ModeReply>>,
 }
 
+/// Example MGM device handler strongly based on the LIS3MDL MEMS device.
 #[derive(new)]
 #[allow(clippy::too_many_arguments)]
-pub struct MgmHandler<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> {
+pub struct MgmHandlerLis3Mdl<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> {
     id: UniqueApidTargetId,
     dev_str: &'static str,
     mode_interface: MpscModeLeafInterface,
@@ -68,26 +74,41 @@ pub struct MgmHandler<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> {
     com_interface: ComInterface,
     shared_mgm_set: Arc<Mutex<MgmData>>,
     #[new(value = "ModeAndSubmode::new(satrs_example::DeviceMode::Off as u32, 0)")]
-    mode: ModeAndSubmode,
+    mode_and_submode: ModeAndSubmode,
     #[new(default)]
     tx_buf: [u8; 12],
     #[new(default)]
     rx_buf: [u8; 12],
     #[new(default)]
-    tm_buf: [u8; 12],
+    tm_buf: [u8; 16],
+    #[new(default)]
     stamp_helper: TimeStampHelper,
 }
 
-impl<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> MgmHandler<ComInterface, TmSender> {
+impl<ComInterface: SpiInterface, TmSender: EcssTmSenderCore>
+    MgmHandlerLis3Mdl<ComInterface, TmSender>
+{
     pub fn periodic_operation(&mut self) {
         self.stamp_helper.update_from_now();
         // Handle requests.
         self.handle_composite_requests();
         self.handle_mode_requests();
+        if self.mode() == DeviceMode::Normal as u32 {
+            log::trace!("polling LIS3MDL sensor {}", self.dev_str);
             // 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.
+            // Actual data begins on the second byte, similarly to how a lot of SPI devices behave.
+            let x_raw = i16::from_be_bytes(self.rx_buf[1..3].try_into().unwrap());
+            let y_raw = i16::from_be_bytes(self.rx_buf[3..5].try_into().unwrap());
+            let z_raw = i16::from_be_bytes(self.rx_buf[5..7].try_into().unwrap());
+            // Simple scaling to retrieve the float value, assuming a sensor resolution of
+            let mut mgm_guard = self.shared_mgm_set.lock().unwrap();
+            mgm_guard.x = x_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR * FIELD_LSB_PER_GAUSS_4_SENS;
+            mgm_guard.y = y_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR * FIELD_LSB_PER_GAUSS_4_SENS;
+            mgm_guard.z = z_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR * FIELD_LSB_PER_GAUSS_4_SENS;
+            drop(mgm_guard);
+        }
     }
 
     pub fn handle_composite_requests(&mut self) {
@@ -134,13 +155,14 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> MgmHandler<ComInter
                     0,
                     Some(self.stamp_helper.stamp()),
                 );
-                // Let's serialize it as JSON for now.. This is a lot simpler than binary
-                // serialization.
                 let mgm_snapshot = *self.shared_mgm_set.lock().unwrap();
-                self.tm_buf[0..4].copy_from_slice(&mgm_snapshot.x.to_be_bytes());
-                self.tm_buf[4..8].copy_from_slice(&mgm_snapshot.y.to_be_bytes());
-                self.tm_buf[8..12].copy_from_slice(&mgm_snapshot.z.to_be_bytes());
-                let hk_tm = PusTmCreator::new(&mut sp_header, sec_header, &self.tm_buf[0..12], true);
+                // Use binary serialization here. We want the data to be tightly packed.
+                self.tm_buf[0] = mgm_snapshot.valid as u8;
+                self.tm_buf[1..5].copy_from_slice(&mgm_snapshot.x.to_be_bytes());
+                self.tm_buf[5..9].copy_from_slice(&mgm_snapshot.y.to_be_bytes());
+                self.tm_buf[9..13].copy_from_slice(&mgm_snapshot.z.to_be_bytes());
+                let hk_tm =
+                    PusTmCreator::new(&mut sp_header, sec_header, &self.tm_buf[0..12], true);
                 self.tm_sender
                     .send_tm(self.id.id(), PusTmVariant::Direct(hk_tm))
                     .expect("failed to send HK TM");
@@ -179,15 +201,15 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> MgmHandler<ComInter
 }
 
 impl<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> ModeProvider
-    for MgmHandler<ComInterface, TmSender>
+    for MgmHandlerLis3Mdl<ComInterface, TmSender>
 {
     fn mode_and_submode(&self) -> ModeAndSubmode {
-        self.mode
+        self.mode_and_submode
     }
 }
 
 impl<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> ModeRequestHandler
-    for MgmHandler<ComInterface, TmSender>
+    for MgmHandlerLis3Mdl<ComInterface, TmSender>
 {
     type Error = ModeError;
     fn start_transition(
@@ -195,14 +217,22 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSenderCore> ModeRequestHandler
         requestor: MessageMetadata,
         mode_and_submode: ModeAndSubmode,
     ) -> Result<(), satrs::mode::ModeError> {
-        self.mode = mode_and_submode;
+        log::info!(
+            "{}: transitioning to mode {:?}",
+            self.dev_str,
+            mode_and_submode
+        );
+        self.mode_and_submode = mode_and_submode;
         self.handle_mode_reached(Some(requestor))?;
         Ok(())
     }
 
     fn announce_mode(&self, _requestor_info: Option<MessageMetadata>, _recursive: bool) {
-        log::info!("{} announcing mode: {:?}", self.dev_str, self.mode);
-        // TODO: Trigger event.
+        log::info!(
+            "{} announcing mode: {:?}",
+            self.dev_str,
+            self.mode_and_submode
+        );
     }
 
     fn handle_mode_reached(

satrs-example/src/main.rs

@@ -27,7 +27,7 @@ use satrs_example::config::{OBSW_SERVER_ADDR, SERVER_PORT};
 use tmtc::PusTcSourceProviderDynamic;
 use udp::DynamicUdpTmHandler;
 
-use crate::acs::mgm::{MgmHandler, MpscModeLeafInterface, SpiDummyInterface};
+use crate::acs::mgm::{MgmHandlerLis3Mdl, MpscModeLeafInterface, SpiDummyInterface};
 use crate::ccsds::CcsdsReceiver;
 use crate::logger::setup_logger;
 use crate::pus::action::{create_action_service_dynamic, create_action_service_static};
@@ -211,7 +211,7 @@ fn static_tmtc_pool_main() {
         reply_tx_to_pus: pus_mode_reply_tx,
         reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx,
     };
-    let mut mgm_handler = MgmHandler::new(
+    let mut mgm_handler = MgmHandlerLis3Mdl::new(
         MGM_HANDLER_0,
         "MGM_0",
         mode_leaf_interface,
@@ -429,7 +429,7 @@ fn dyn_tmtc_pool_main() {
         reply_tx_to_pus: pus_mode_reply_tx,
         reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx,
     };
-    let mut mgm_handler = MgmHandler::new(
+    let mut mgm_handler = MgmHandlerLis3Mdl::new(
         MGM_HANDLER_0,
         "MGM_0",
         mode_leaf_interface,

satrs/src/mode.rs

@@ -179,6 +179,14 @@ impl From<GenericTargetedMessagingError> for ModeError {
 
 pub trait ModeProvider {
     fn mode_and_submode(&self) -> ModeAndSubmode;
+
+    fn mode(&self) -> Mode {
+        self.mode_and_submode().mode()
+    }
+
+    fn submode(&self) -> Submode {
+        self.mode_and_submode().submode()
+    }
 }
 
 pub trait ModeRequestHandler: ModeProvider {