diff --git a/satrs-example/src/acs/mgm.rs b/satrs-example/src/acs/mgm.rs
index 81bb1f7..f245325 100644
--- a/satrs-example/src/acs/mgm.rs
+++ b/satrs-example/src/acs/mgm.rs
@@ -5,11 +5,14 @@ use satrs::spacepackets::ecss::hk;
 use satrs::spacepackets::ecss::tm::{PusTmCreator, PusTmSecondaryHeader};
 use satrs::spacepackets::SpHeader;
 use satrs_example::{DeviceMode, TimestampHelper};
-use satrs_minisim::acs::lis3mdl::{FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR};
+use satrs_minisim::acs::lis3mdl::{
+    MgmLis3MdlReply, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR,
+};
 use satrs_minisim::acs::MgmRequestLis3Mdl;
-use satrs_minisim::{SimReply, SimRequest};
+use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest};
 use std::sync::mpsc::{self};
 use std::sync::{Arc, Mutex};
+use std::time::Duration;
 
 use satrs::mode::{
     ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequest, ModeRequestHandler,
@@ -62,13 +65,20 @@ impl SpiInterface for SpiSimInterface {
     type Error = ();
 
     // Right now, we only support requesting sensor data and not configuration of the sensor.
-    fn transfer(&mut self, _tx: &[u8], _rx: &mut [u8]) -> Result<(), Self::Error> {
+    fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
         let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData;
         self.sim_request_tx
             .send(SimRequest::new_with_epoch_time(mgm_sensor_request))
             .expect("failed to send request");
-        self.sim_reply_rx.recv().expect("reply timeout");
-        // TODO: Write the sensor data to the raw buffer.
+        let sim_reply = self
+            .sim_reply_rx
+            .recv_timeout(Duration::from_millis(100))
+            .expect("reply timeout");
+        let sim_reply_lis3 =
+            MgmLis3MdlReply::from_sim_message(&sim_reply).expect("failed to parse LIS3 reply");
+        rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.x.to_le_bytes());
+        rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.y.to_be_bytes());
+        rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&sim_reply_lis3.raw.z.to_be_bytes());
         Ok(())
     }
 }
@@ -99,8 +109,8 @@ pub struct MgmData {
 
 pub struct MpscModeLeafInterface {
     pub request_rx: mpsc::Receiver<GenericMessage<ModeRequest>>,
-    pub reply_tx_to_pus: mpsc::Sender<GenericMessage<ModeReply>>,
-    pub reply_tx_to_parent: mpsc::Sender<GenericMessage<ModeReply>>,
+    pub reply_to_pus_tx: mpsc::Sender<GenericMessage<ModeReply>>,
+    pub reply_to_parent_tx: mpsc::Sender<GenericMessage<ModeReply>>,
 }
 
 /// Example MGM device handler strongly based on the LIS3MDL MEMS device.
@@ -110,8 +120,8 @@ pub struct MgmHandlerLis3Mdl<ComInterface: SpiInterface, TmSender: EcssTmSender>
     id: UniqueApidTargetId,
     dev_str: &'static str,
     mode_interface: MpscModeLeafInterface,
-    composite_request_receiver: mpsc::Receiver<GenericMessage<CompositeRequest>>,
-    hk_reply_sender: mpsc::Sender<GenericMessage<HkReply>>,
+    composite_request_rx: mpsc::Receiver<GenericMessage<CompositeRequest>>,
+    hk_reply_tx: mpsc::Sender<GenericMessage<HkReply>>,
     tm_sender: TmSender,
     com_interface: ComInterface,
     shared_mgm_set: Arc<Mutex<MgmData>>,
@@ -135,43 +145,13 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> MgmHandlerLis3Mdl<ComIn
         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[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
-                &mut self.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
-            );
-            assert!(result.is_ok());
-            // Actual data begins on the second byte, similarly to how a lot of SPI devices behave.
-            let x_raw = i16::from_le_bytes(
-                self.rx_buf[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2]
-                    .try_into()
-                    .unwrap(),
-            );
-            let y_raw = i16::from_le_bytes(
-                self.rx_buf[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
-                    .try_into()
-                    .unwrap(),
-            );
-            let z_raw = i16::from_le_bytes(
-                self.rx_buf[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2]
-                    .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 as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
-            mgm_guard.y =
-                y_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
-            mgm_guard.z =
-                z_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
-            drop(mgm_guard);
+            self.poll_sensor();
         }
     }
 
     pub fn handle_composite_requests(&mut self) {
         loop {
-            match self.composite_request_receiver.try_recv() {
+            match self.composite_request_rx.try_recv() {
                 Ok(ref msg) => match &msg.message {
                     CompositeRequest::Hk(hk_request) => {
                         self.handle_hk_request(&msg.requestor_info, hk_request)
@@ -199,7 +179,7 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> MgmHandlerLis3Mdl<ComIn
     pub fn handle_hk_request(&mut self, requestor_info: &MessageMetadata, hk_request: &HkRequest) {
         match hk_request.variant {
             HkRequestVariant::OneShot => {
-                self.hk_reply_sender
+                self.hk_reply_tx
                     .send(GenericMessage::new(
                         *requestor_info,
                         HkReply::new(hk_request.unique_id, HkReplyVariant::Ack),
@@ -234,6 +214,38 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> MgmHandlerLis3Mdl<ComIn
         }
     }
 
+    pub fn poll_sensor(&mut self) {
+        // Communicate with the device. This is actually how to read the data from the LIS3 device
+        // SPI interface.
+        let result = self.com_interface.transfer(
+            &self.tx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
+            &mut self.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
+        );
+        assert!(result.is_ok());
+        let x_raw = i16::from_le_bytes(
+            self.rx_buf[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2]
+                .try_into()
+                .unwrap(),
+        );
+        let y_raw = i16::from_le_bytes(
+            self.rx_buf[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
+                .try_into()
+                .unwrap(),
+        );
+        let z_raw = i16::from_le_bytes(
+            self.rx_buf[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2]
+                .try_into()
+                .unwrap(),
+        );
+        // Simple scaling to retrieve the float value, assuming the best sensor resolution.
+        let mut mgm_guard = self.shared_mgm_set.lock().unwrap();
+        mgm_guard.x = x_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
+        mgm_guard.y = y_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
+        mgm_guard.z = z_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
+        mgm_guard.valid = true;
+        drop(mgm_guard);
+    }
+
     pub fn handle_mode_requests(&mut self) {
         loop {
             // TODO: Only allow one set mode request per cycle?
@@ -284,6 +296,9 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> ModeRequestHandler
             mode_and_submode
         );
         self.mode_and_submode = mode_and_submode;
+        if mode_and_submode.mode() == DeviceMode::Off as u32 {
+            self.shared_mgm_set.lock().unwrap().valid = false;
+        }
         self.handle_mode_reached(Some(requestor))?;
         Ok(())
     }
@@ -326,7 +341,7 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> ModeRequestHandler
             );
         }
         self.mode_interface
-            .reply_tx_to_pus
+            .reply_to_pus_tx
             .send(GenericMessage::new(requestor, reply))
             .map_err(|_| GenericTargetedMessagingError::Send(GenericSendError::RxDisconnected))?;
         Ok(())
@@ -343,5 +358,168 @@ impl<ComInterface: SpiInterface, TmSender: EcssTmSender> ModeRequestHandler
 
 #[cfg(test)]
 mod tests {
-    // TODO: Add some basic tests for the modes of the device.
+    use std::sync::{atomic::AtomicU32, mpsc, Arc, Mutex};
+
+    use satrs::{
+        mode::{ModeReply, ModeRequest},
+        request::{GenericMessage, UniqueApidTargetId},
+        tmtc::PacketAsVec,
+    };
+    use satrs_example::config::components::Apid;
+    use satrs_minisim::acs::lis3mdl::MgmLis3RawValues;
+
+    use crate::{pus::hk::HkReply, requests::CompositeRequest};
+
+    use super::*;
+
+    pub struct TestInterface {
+        pub call_count: Arc<AtomicU32>,
+        pub next_mgm_data: Arc<Mutex<MgmLis3RawValues>>,
+    }
+
+    impl TestInterface {
+        pub fn new(
+            call_count: Arc<AtomicU32>,
+            next_mgm_data: Arc<Mutex<MgmLis3RawValues>>,
+        ) -> Self {
+            Self {
+                call_count,
+                next_mgm_data,
+            }
+        }
+    }
+
+    impl SpiInterface for TestInterface {
+        type Error = ();
+
+        fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
+            let mgm_data = *self.next_mgm_data.lock().unwrap();
+            rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.x.to_le_bytes());
+            rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.y.to_be_bytes());
+            rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&mgm_data.z.to_be_bytes());
+            self.call_count
+                .fetch_add(1, std::sync::atomic::Ordering::Relaxed);
+            Ok(())
+        }
+    }
+
+    pub struct MgmTestbench {
+        pub spi_interface_call_count: Arc<AtomicU32>,
+        pub next_mgm_data: Arc<Mutex<MgmLis3RawValues>>,
+        pub mode_request_tx: mpsc::Sender<GenericMessage<ModeRequest>>,
+        pub mode_reply_rx_to_pus: mpsc::Receiver<GenericMessage<ModeReply>>,
+        pub mode_reply_rx_to_parent: mpsc::Receiver<GenericMessage<ModeReply>>,
+        pub composite_request_tx: mpsc::Sender<GenericMessage<CompositeRequest>>,
+        pub hk_reply_rx: mpsc::Receiver<GenericMessage<HkReply>>,
+        pub tm_rx: mpsc::Receiver<PacketAsVec>,
+        pub handler: MgmHandlerLis3Mdl<TestInterface, mpsc::Sender<PacketAsVec>>,
+    }
+
+    impl MgmTestbench {
+        pub fn new() -> Self {
+            let (request_tx, request_rx) = mpsc::channel();
+            let (reply_tx_to_pus, reply_rx_to_pus) = mpsc::channel();
+            let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::channel();
+            let mode_interface = MpscModeLeafInterface {
+                request_rx,
+                reply_to_pus_tx: reply_tx_to_pus,
+                reply_to_parent_tx: reply_tx_to_parent,
+            };
+            let (composite_request_tx, composite_request_rx) = mpsc::channel();
+            let (hk_reply_tx, hk_reply_rx) = mpsc::channel();
+            let (tm_tx, tm_rx) = mpsc::channel::<PacketAsVec>();
+            let shared_mgm_set = Arc::default();
+            let next_mgm_data = Arc::new(Mutex::default());
+            let spi_interface_call_count = Arc::new(AtomicU32::new(0));
+            let test_interface =
+                TestInterface::new(spi_interface_call_count.clone(), next_mgm_data.clone());
+            Self {
+                mode_request_tx: request_tx,
+                mode_reply_rx_to_pus: reply_rx_to_pus,
+                mode_reply_rx_to_parent: reply_rx_to_parent,
+                composite_request_tx,
+                tm_rx,
+                hk_reply_rx,
+                spi_interface_call_count,
+                next_mgm_data,
+                handler: MgmHandlerLis3Mdl::new(
+                    UniqueApidTargetId::new(Apid::Acs as u16, 1),
+                    "test-mgm",
+                    mode_interface,
+                    composite_request_rx,
+                    hk_reply_tx,
+                    tm_tx,
+                    test_interface,
+                    shared_mgm_set,
+                ),
+            }
+        }
+    }
+
+    #[test]
+    fn test_basic_handler() {
+        let mut testbench = MgmTestbench::new();
+        assert_eq!(
+            testbench
+                .spi_interface_call_count
+                .load(std::sync::atomic::Ordering::Relaxed),
+            0
+        );
+        assert_eq!(
+            testbench.handler.mode_and_submode().mode(),
+            DeviceMode::Off as u32
+        );
+        assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16);
+        testbench.handler.periodic_operation();
+        // Handler is OFF, no changes expected.
+        assert_eq!(
+            testbench
+                .spi_interface_call_count
+                .load(std::sync::atomic::Ordering::Relaxed),
+            0
+        );
+        assert_eq!(
+            testbench.handler.mode_and_submode().mode(),
+            DeviceMode::Off as u32
+        );
+        assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16);
+    }
+
+    #[test]
+    fn test_normal_handler() {
+        let mut testbench = MgmTestbench::new();
+        testbench
+            .mode_request_tx
+            .send(GenericMessage::new(
+                MessageMetadata::new(0, PUS_MODE_SERVICE.id()),
+                ModeRequest::SetMode(ModeAndSubmode::new(DeviceMode::Normal as u32, 0)),
+            ))
+            .expect("failed to send mode request");
+        testbench.handler.periodic_operation();
+        assert_eq!(
+            testbench.handler.mode_and_submode().mode(),
+            DeviceMode::Normal as u32
+        );
+        assert_eq!(testbench.handler.mode_and_submode().submode(), 0);
+        let mode_reply = testbench
+            .mode_reply_rx_to_pus
+            .try_recv()
+            .expect("no mode reply generated");
+        match mode_reply.message {
+            ModeReply::ModeReply(mode) => {
+                assert_eq!(mode.mode(), DeviceMode::Normal as u32);
+                assert_eq!(mode.submode(), 0);
+            }
+            _ => panic!("unexpected mode reply"),
+        }
+        // The device should have been polled once.
+        assert_eq!(
+            testbench
+                .spi_interface_call_count
+                .load(std::sync::atomic::Ordering::Relaxed),
+            1
+        );
+        // TODO: Check shared MGM set. The field values should be 0, but the entry should be valid.
+        // TODO: Set non-zero raw values for the next polled MGM set.
+    }
 }
diff --git a/satrs-example/src/main.rs b/satrs-example/src/main.rs
index a0babb8..80ce229 100644
--- a/satrs-example/src/main.rs
+++ b/satrs-example/src/main.rs
@@ -209,8 +209,8 @@ fn static_tmtc_pool_main() {
     let shared_mgm_set = Arc::default();
     let mode_leaf_interface = MpscModeLeafInterface {
         request_rx: mgm_handler_mode_rx,
-        reply_tx_to_pus: pus_mode_reply_tx,
-        reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx,
+        reply_to_pus_tx: pus_mode_reply_tx,
+        reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx,
     };
 
     let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() {
@@ -448,8 +448,8 @@ fn dyn_tmtc_pool_main() {
     let shared_mgm_set = Arc::default();
     let mode_leaf_interface = MpscModeLeafInterface {
         request_rx: mgm_handler_mode_rx,
-        reply_tx_to_pus: pus_mode_reply_tx,
-        reply_tx_to_parent: mgm_handler_mode_reply_to_parent_tx,
+        reply_to_pus_tx: pus_mode_reply_tx,
+        reply_to_parent_tx: mgm_handler_mode_reply_to_parent_tx,
     };
 
     let mgm_spi_interface = if let Some(sim_client) = opt_sim_client.as_mut() {
diff --git a/satrs-minisim/src/lib.rs b/satrs-minisim/src/lib.rs
index 1beb32b..361508e 100644
--- a/satrs-minisim/src/lib.rs
+++ b/satrs-minisim/src/lib.rs
@@ -247,7 +247,7 @@ pub mod acs {
         pub const FIELD_LSB_PER_GAUSS_12_SENS: f32 = 1.0 / 2281.0;
         pub const FIELD_LSB_PER_GAUSS_16_SENS: f32 = 1.0 / 1711.0;
 
-        #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)]
+        #[derive(Default, Debug, Copy, Clone, PartialEq, Serialize, Deserialize)]
         pub struct MgmLis3RawValues {
             pub x: i16,
             pub y: i16,