updates

Reviewed-on: #1

.idea/runConfigurations/Cross_Remote_Debug.xml

@@ -1,6 +1,6 @@
 <component name="ProjectRunConfigurationManager">
   <configuration default="false" name="Cross Remote Debug" type="com.jetbrains.cidr.remote.gdbserver.type" factoryName="com.jetbrains.cidr.remote.gdbserver.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="eurosim-obsw" TARGET_NAME="Cross" CONFIG_NAME="Cross" version="1" RUN_PATH="$PROJECT_DIR$/target/armv7-unknown-linux-gnueabihf/debug/eurosim-obsw">
-    <custom-gdb-server version="1" gdb-connect="obc.local:1234" executable="" warmup-ms="0" download-type="ALWAYS" sshConfigName="koesterl@obc.local:22 password" uploadFile="/tmp/CLion/debug/eurosim-obsw" defaultGdbServerArgs=":1234 /tmp/CLion/debug/eurosim-obsw">
+    <custom-gdb-server version="1" gdb-connect="192.168.1.116:1234" executable="" warmup-ms="0" download-type="ALWAYS" sshConfigName="koesterl@192.168.1.116:22 password" uploadFile="/tmp/CLion/debug/eurosim-obsw" defaultGdbServerArgs=":1234 /tmp/CLion/debug/eurosim-obsw">
       <debugger kind="GDB" isBundled="true" />
     </custom-gdb-server>
     <method v="2">

Cargo.lock

@@ -288,6 +288,7 @@ dependencies = [
 name = "eurosim-obsw"
 version = "0.1.0"
 dependencies = [
+ "byteorder",
  "chrono",
  "embedded-can",
  "fern",
@@ -812,9 +813,9 @@ dependencies = [
 
 [[package]]
 name = "spacepackets"
-version = "0.5.3"
+version = "0.5.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "81541fd89a5bc02845a849895d6ed1721235b3eac26fc77010f0a05f53bc4e8a"
+checksum = "9313bf066de3ae704ea83584fc65b62434249efe57ae2dcde3e2cffe4ba62648"
 dependencies = [
  "chrono",
  "crc",

Cargo.toml

@@ -26,6 +26,8 @@ strum_macros = "0.24"
 num = "0.4"
 num-derive = "0.3"
 num-traits = "0.2"
+byteorder = "1.4"
+
 
 [dependencies.socketcan]
 git = "https://github.com/socketcan-rs/socketcan-rs.git"

output.log

@@ -0,0 +1,43 @@
+[2023-02-15][09:14:42][main][INFO] Starting TMTC task
+[2023-02-15][09:14:42][main][INFO] Starting power task
+[2023-02-15][09:14:42][main][INFO] Starting AOCS task
+[2023-02-15][09:16:43][main][INFO] Starting TMTC task
+[2023-02-15][09:16:43][main][INFO] Starting power task
+[2023-02-15][09:16:43][main][INFO] Starting AOCS task
+[2023-02-15][11:05:33][main][INFO] Starting TMTC task
+[2023-02-15][11:05:33][main][INFO] Starting CAN Socket listening task
+[2023-02-15][11:05:33][main][INFO] Starting CAN Socket writing task
+[2023-02-15][11:05:33][main][INFO] Starting power task
+[2023-02-15][11:05:33][main][INFO] Starting AOCS task
+[2023-02-15][11:10:52][main][INFO] Starting TMTC task
+[2023-02-15][11:10:52][main][INFO] Starting power task
+[2023-02-15][11:10:52][main][INFO] Starting AOCS task
+[2023-02-15][11:11:09][main][INFO] Starting TMTC task
+[2023-02-15][11:11:09][main][INFO] Starting power task
+[2023-02-15][11:11:09][main][INFO] Starting AOCS task
+[2023-02-15][11:11:37][main][INFO] Starting TMTC task
+[2023-02-15][11:11:37][main][INFO] Starting power task
+[2023-02-15][11:11:37][main][INFO] Starting AOCS task
+[2023-02-15][14:42:44][main][INFO] Starting TMTC task
+[2023-02-15][14:42:44][main][INFO] Starting power task
+[2023-02-15][14:42:44][main][INFO] Starting Payload Handling task
+[2023-02-15][14:42:44][main][INFO] Starting TM funnel task
+[2023-02-15][14:42:44][main][INFO] Starting AOCS task
+[2023-02-15][14:43:13][main][INFO] Running DemoSat OBSW
+[2023-02-15][14:43:13][main][INFO] Starting TMTC task
+[2023-02-15][14:43:13][main][INFO] Starting power task
+[2023-02-15][14:43:13][main][INFO] Starting Payload Handling task
+[2023-02-15][14:43:13][main][INFO] Starting TM funnel task
+[2023-02-15][14:43:13][main][INFO] Starting AOCS task
+[2023-02-15][14:48:12][main][INFO] Running DemoSat OBSW!
+[2023-02-15][14:48:12][main][INFO] Starting TMTC task
+[2023-02-15][14:48:12][main][INFO] Starting power task
+[2023-02-15][14:48:12][main][INFO] Starting Payload Handling task
+[2023-02-15][14:48:12][main][INFO] Starting TM funnel task
+[2023-02-15][14:48:12][main][INFO] Starting AOCS task
+[2023-02-16][12:46:02][main][INFO] Running DemoSat OBSW!
+[2023-02-16][12:46:02][main][INFO] Starting TMTC task
+[2023-02-16][12:46:02][main][INFO] Starting power task
+[2023-02-16][12:46:02][main][INFO] Starting Payload Handling task
+[2023-02-16][12:46:02][main][INFO] Starting TM funnel task
+[2023-02-16][12:46:02][main][INFO] Starting AOCS task

pyclient/main copy.py

@@ -7,6 +7,8 @@ import sys
 import time
 from typing import Optional
 import datetime
+import json
+import pprint
 
 import tmtccmd
 from spacepackets.ecss import PusTelemetry, PusTelecommand, PusVerificator
@@ -110,6 +112,7 @@ class SatRsConfigHook(HookBase):
 
         srv_11 = OpCodeEntry()
         srv_11.add("0", "Scheduled TC Test")
+        srv_11.add("1", "Scheduled Camera Request TC")
         defs.add_service(
             name=CoreServiceList.SERVICE_11,
             info="PUS Service 11 TC Scheduling",
@@ -168,13 +171,15 @@ class PusHandler(SpecificApidHandlerBase):
                 self.verif_wrapper.log_to_file(tm_packet, res)
             dedicated_handler = True
         if service == 3:
-            _LOGGER.info("No handling for HK packets implemented")
-            _LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]")
+            # _LOGGER.info("No handling for HK packets implemented")
+            # _LOGGER.info(f"Raw packet: 0x[{packet.hex(sep=',')}]")            
             pus_tm = PusTelemetry.unpack(packet, time_reader=CdsShortTimestamp.empty())
             if pus_tm.subservice == 25:
                 if len(pus_tm.source_data) < 8:
                     raise ValueError("No addressable ID in HK packet")
-                json_str = pus_tm.source_data[8:]
+                json_str = str(pus_tm.source_data[8:].decode('utf8'))
+                json_object = json.loads(json_str)
+                pprint.pprint(json_object)
             dedicated_handler = True
         if service == 5:
             tm_packet = Service5Tm.unpack(packet, time_reader=CdsShortTimestamp.empty())
@@ -218,6 +223,9 @@ def read_addressable_id(data: bytes) -> tuple[int, int]:
     return (target_id, set_id)
 
 
+class CustomServiceList(enum.IntEnum):
+    ACS = "acs"
+
 class RequestTargetId(enum.IntEnum):
     ACS = 1
     PLD = 2
@@ -286,18 +294,35 @@ class TcHandler(TcHandlerBase):
             ):
                 q.add_log_cmd("Sending PUS ping telecommand")
                 return q.add_pus_tc(PusTelecommand(service=17, subservice=1))
+            if service == CustomServiceList.ACS:
+                if op_code == "set_mode":
+                    q.add_log_cmd()
+                    q.add_pus_tc()
             if service == CoreServiceList.SERVICE_11:
-                q.add_log_cmd("Sending PUS scheduled TC telecommand")
-                crt_time = CdsShortTimestamp.from_now()
-                time_stamp = crt_time + datetime.timedelta(seconds=10)
-                time_stamp = time_stamp.pack()
-                return q.add_pus_tc(
-                    create_time_tagged_cmd(
-                        time_stamp,
-                        PusTelecommand(service=17, subservice=1),
-                        apid=EXAMPLE_PUS_APID,
+                if op_code == "0":
+                    q.add_log_cmd("Sending PUS scheduled TC telecommand")
+                    crt_time = CdsShortTimestamp.from_now()
+                    time_stamp = crt_time + datetime.timedelta(seconds=10)
+                    time_stamp = time_stamp.pack()
+                    return q.add_pus_tc(
+                        create_time_tagged_cmd(
+                            time_stamp,
+                            PusTelecommand(service=17, subservice=1),
+                            apid=EXAMPLE_PUS_APID,
+                        )
+                    )
+                if op_code == "1":
+                    q.add_log_cmd("Sending PUS scheduled TC telecommand")
+                    crt_time = CdsShortTimestamp.from_now()
+                    time_stamp = crt_time + datetime.timedelta(seconds=10)
+                    time_stamp = time_stamp.pack()
+                    return q.add_pus_tc(
+                        create_time_tagged_cmd(
+                            time_stamp,
+                            PusTelecommand(service=8, subservice=1, app_data=make_target_id(RequestTargetId.PLD)),
+                            apid=EXAMPLE_PUS_APID,
+                        )
                     )
-                )
             if service == CoreServiceList.SERVICE_8:
                 q.add_log_cmd("Sending PUS action request telecommand")
                 return q.add_pus_tc(

pyclient/tmtc_conf.json

@@ -1,6 +1,7 @@
 {
     "com_if": "udp",
-    "tcpip_udp_ip_addr": "169.254.7.6",
+    "tcpip_udp_ip_addr": "192.168.1.116",
+    "tcpip_udp_ip_addr_windows": "192.168.1.5",
     "tcpip_udp_port": 7301,
     "tcpip_udp_recv_max_size": 1500
 }

src/aocs_handler.rs

@@ -0,0 +1,206 @@
+use std::sync::{Arc, Mutex};
+use crate::can_ids::{DeviceId, PackageId, PackageModel};
+use crate::hk::HkRequest;
+use crate::power_handler::{DeviceState, PowerSwitcher};
+use crate::requests::{Request, RequestWithToken};
+use satrs_core::power::SwitchId;
+use std::sync::mpsc::{Receiver, Sender};
+use serde::{Deserialize, Serialize};
+use byteorder::{LittleEndian, ByteOrder};
+use crate::can_ids::PackageId::AOCSDataRequestMGM1;
+
+pub enum AocsSensorMode {
+    Idle,
+    SendingData,
+}
+
+pub trait AocsSensorHandler {
+    type Error;
+
+    fn get_package_id(&mut self) -> Result<PackageId, Self::Error>;
+    fn send_message(&mut self, id: PackageId, buf: &[u8]) -> Result<(), Self::Error>;
+
+    fn enable_sensor_data_generation(&mut self) -> Result<(), Self::Error> {
+        let id = self.get_package_id()?;
+        self.send_message(id, &[1])
+    }
+
+    fn disable_sensor_data_generation(&mut self) -> Result<(), Self::Error> {
+        let id = self.get_package_id()?;
+        self.send_message(id, &[0])
+    }
+
+    fn request_sensor_data_oneshot(&mut self) -> Result<(), Self::Error> {
+        let id = self.get_package_id()?;
+        self.send_message(id, &[2])
+    }
+}
+
+#[derive(Debug, Copy, Clone, Default, Serialize, Deserialize)]
+pub struct MGMData {
+    axis_1: f64,
+    axis_2: f64,
+    axis_3: f64,
+}
+
+impl MGMData {
+    pub fn from_floats(axis_1: f64,
+                       axis_2: f64,
+                       axis_3: f64,
+    ) -> MGMData {
+        MGMData{axis_1, axis_2, axis_3}
+    }
+
+    pub fn new() -> MGMData { MGMData{axis_1: 0.0, axis_2: 0.0, axis_3: 0.0} }
+
+    pub fn update(&mut self,
+                  axis_1: f64,
+                  axis_2: f64,
+                  axis_3: f64
+    ) {
+        self.axis_1 = axis_1;
+        self.axis_2 = axis_2;
+        self.axis_3 = axis_3;
+    }
+
+    pub fn to_array(&self) -> [f64; 3] {
+        [self.axis_1, self.axis_2, self.axis_3]
+    }
+
+    pub fn to_tuple(&self) -> (f64, f64, f64) {
+        (self.axis_1, self.axis_2, self.axis_3)
+    }
+}
+
+pub struct MGMHandler {
+    power_switcher: PowerSwitcher,
+    device_id: DeviceId,
+    switch_id: SwitchId,
+    device_state: DeviceState,
+    can_tx: Sender<PackageModel>,
+    can_rx: Receiver<PackageModel>,
+    mode: AocsSensorMode,
+    mode_rx: Receiver<AocsSensorMode>,
+    action_rx: Receiver<RequestWithToken>,
+    mgm_data: Arc<Mutex<MGMData>>,
+    sensor_data_enabled: bool,
+}
+
+impl AocsSensorHandler for MGMHandler {
+    type Error = ();
+
+    fn get_package_id(&mut self) -> Result<PackageId, Self::Error> {
+        return match self.device_id {
+            DeviceId::MGM1 => Ok(PackageId::AOCSDataRequestMGM1),
+            DeviceId::MGM2 => Ok(PackageId::AOCSDataRequestMGM2),
+            DeviceId::MGM3 => Ok(PackageId::AOCSDataRequestMGM3),
+            DeviceId::MGM4 => Ok(PackageId::AOCSDataRequestMGM4),
+            _ => Err(()),
+        };
+    }
+
+    fn send_message(&mut self, id: PackageId, buf: &[u8]) -> Result<(), Self::Error> {
+        self.can_tx.send(PackageModel::new(id, buf).unwrap()).unwrap();
+        return Ok(());
+    }
+}
+
+impl MGMHandler {
+    pub fn new(
+        power_switcher: PowerSwitcher,
+        device_id: DeviceId,
+        can_tx: Sender<PackageModel>,
+        can_rx: Receiver<PackageModel>,
+        mode_rx: Receiver<AocsSensorMode>,
+        action_rx: Receiver<RequestWithToken>,
+    ) -> MGMHandler {
+        let switch_id = device_id as u16;
+        MGMHandler {
+            power_switcher,
+            device_id,
+            switch_id,
+            device_state: DeviceState::Off,
+            can_tx,
+            can_rx,
+            mode: AocsSensorMode::Idle,
+            mode_rx,
+            action_rx,
+            mgm_data: Arc::new(Mutex::new(MGMData::new())),
+            sensor_data_enabled: false,
+        }
+    }
+
+    pub fn get_data_ref(&mut self) -> Arc<Mutex<MGMData>> {
+        self.mgm_data.clone()
+    }
+
+    pub fn periodic_op(&mut self) {
+        self.update_mode();
+        self.handle_requests();
+        self.read_sensor_data();
+    }
+
+    pub fn update_mode(&mut self) {
+        if self.device_state == DeviceState::On {
+            if let Ok(mode) = self.mode_rx.try_recv() {
+                self.mode = mode;
+            }
+        }
+    }
+
+    pub fn handle_requests(&mut self) {
+        if self.device_state == DeviceState::On {
+            if let Ok(request) = self.action_rx.try_recv() {
+                match request.0 {
+                    Request::HkRequest(hk_req) => {
+                        self.handle_hk_request(hk_req);
+                    }
+                    Request::ActionRequest(_action_request) => {
+                        //self.handle_action_request(action_request);
+                    }
+                }
+            }
+        }
+    }
+
+    pub fn handle_hk_request(&mut self, hk_req: HkRequest) {
+        match hk_req {
+            HkRequest::OneShot(_) => {
+                self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[2]).unwrap()).unwrap();
+            }
+            HkRequest::Enable(_) => {
+                if !self.sensor_data_enabled {
+                    self.sensor_data_enabled = true;
+                    self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[1]).unwrap()).unwrap();
+                }
+            }
+            HkRequest::Disable(_) => {
+                if self.sensor_data_enabled {
+                    self.sensor_data_enabled = false;
+                    self.can_tx.send(PackageModel::new(PackageId::AOCSDataRequestMGM1, &[0]).unwrap()).unwrap();
+                }
+            }
+            HkRequest::ModifyCollectionInterval(_, _) => {}
+        }
+    }
+
+    pub fn read_sensor_data(&mut self) {
+        if let Ok(package) = self.can_rx.try_recv() {
+            let float_data = self.decode_sensor_data(package.data());
+            if let Ok(mut mgm_data) = self.mgm_data.lock() {
+                match package.package_id() {
+                    PackageId::AOCSDataMGM1 => { mgm_data.axis_1 = float_data }
+                    PackageId::AOCSDataMGM2 => { mgm_data.axis_2 = float_data }
+                    PackageId::AOCSDataMGM3 => { mgm_data.axis_3 = float_data }
+                    _ => {}
+                }
+            }
+
+        }
+    }
+
+    pub fn decode_sensor_data(&mut self, buf: &[u8]) -> f64 {
+        LittleEndian::read_f64(&buf)
+    }
+    //pub fn handle_action_request(&mut self, action_request: ActionRequest) {}
+}

src/can.rs

@@ -1,23 +1,13 @@
-#![allow(dead_code)]
-#![allow(unused_variables)]
-#![allow(unused_imports)]
-
-use crate::pcdu::DeviceState;
-
 use crate::can_ids::{
-    can_id_to_package_id, package_id_to_can_id, value_to_package_id, DeviceId, PackageId,
+    can_id_to_package_id, package_id_to_can_id, DeviceId, PackageId,
     PackageModel, SenderReceiverThread, ThreadId,
 };
 use embedded_can::{self, Frame};
-use log::{debug, error, info, trace, warn};
-use serde::{Deserialize, Serialize};
-use serde_json;
+use log::{debug, warn};
 use socketcan::{errors, frame, socket, CanFrame, Socket};
 use std::collections::HashMap;
-use std::ffi::c_void;
 use std::io;
-use std::mem::size_of;
-use std::sync::mpsc::Sender;
+use std::sync::mpsc::{Receiver, Sender};
 
 pub struct CanRxHandler {
     interface: &'static str,
@@ -49,21 +39,11 @@ impl CanRxHandler {
         }
     }
 
-    /*
-    pub fn new_socket_with_filter(
-        interface: &'static str,
-        can_id_to_sender_id: HashMap<embedded_can::Id, u32>,
-        can_senders: HashMap<u32, Sender<Vec<u8>>>,
-        can_filters: &[socket::CanFilter],
-    ) -> Result<CanRxHandler, ()> {
-        let can_wrapper = Self::new_socket(interface, can_id_to_sender_id, can_senders)?;
-        let filter_result = can_wrapper.socket.set_filters(can_filters);
-        if let Err(e) = filter_result {
-            warn!("Can Bus set filter error: {}", e);
+    pub fn process_incoming(&mut self) {
+        if let Some(frame) = self.rx_socket() {
+            self.forward_frame(frame);
         }
-        Ok(can_wrapper)
     }
-    */
 
     pub fn read_frame(&self) -> io::Result<frame::CanFrame> {
         let frame = self.socket.read_frame();
@@ -75,20 +55,13 @@ impl CanRxHandler {
 
     pub fn rx_socket(&self) -> Option<CanFrame> {
         let frame = self.socket.read_frame().ok()?;
-        info!("Can Frame read: {:?}.", frame);
+        debug!("Can Frame read: {:?}.", frame);
         return Some(frame);
-        /*
-        if let Ok(frame) = frame {
-            println!("Frame received: {:?}", frame);
-            return Some(frame);
-        }
-        None
-         */
     }
 
     pub fn forward_frame(&self, frame: CanFrame) {
         let frame_id = can_id_to_package_id(frame.id());
-        info!("Frame forwarding with id: {:?}", frame_id);
+        debug!("Frame forwarding with id: {:?}", frame_id);
         if let Some(frame_id) = frame_id {
             if self.package_map.contains_key(&frame_id) {
                 let value = self.package_map.get(&frame_id).unwrap();
@@ -106,20 +79,73 @@ impl CanRxHandler {
             }
         }
     }
-
-    /*
-    pub fn set_filter(&self, filters: &[socket::CanFilter]) -> io::Result<()> {
-        info!("Setting filter with filter {:?}", filters);
-        let result = self.socket.set_filters(filters);
-        if let Err(e) = result {
-            warn!("Can bus socket filter set error: {}", e);
-        }
-        Ok(())
-    }
-    */
 }
 
 pub struct CanTxHandler {
+    interface: &'static str,
+    socket: socket::CanSocket,
+    package_map: HashMap<PackageId, SenderReceiverThread>,
+    message_receiver: Receiver<PackageModel>,
+}
+
+impl CanTxHandler {
+    pub fn new_socket(
+        interface: &'static str,
+        package_map: HashMap<PackageId, SenderReceiverThread>,
+        message_receiver: Receiver<PackageModel>,
+    ) -> Result<CanTxHandler, ()> {
+        let socket = socket::CanSocket::open(&interface);
+
+        if let Ok(socket) = socket {
+            socket.filter_drop_all().unwrap(); // tx nodes cannot receive data
+            Ok(CanTxHandler {
+                interface,
+                socket,
+                package_map,
+                message_receiver,
+            })
+        } else {
+            Err(())
+        }
+    }
+
+    pub fn process_incoming(&mut self) {
+        if let Ok(package) = self.message_receiver.recv() {
+            self.tx_socket(package.package_id(), package.data());
+        }
+    }
+
+    pub fn tx_socket(&self, package_id: PackageId, data: &[u8]) {
+        if self.package_map.contains_key(&package_id) {
+            let value = self.package_map.get(&package_id).unwrap();
+            if value.get_sender() == DeviceId::OBC {
+                if data.len() <= 8 {
+                    let frame_id = package_id_to_can_id(&package_id);
+                    let frame = CanFrame::new(frame_id, data);
+                    if let Some(frame) = frame {
+                        self.socket
+                            .write_frame(&frame)
+                            .expect("Error writing frame.");
+                    }
+                } else {
+                    warn!(
+                        "Message dismissed, data length ({:?}) exceeds 8 bytes",
+                        data.len()
+                    );
+                }
+            } else {
+                warn!(
+                    "Message dismissed, wrong sender id: {:?}",
+                    value.get_sender()
+                );
+            }
+        } else {
+            warn!("Message dismissed, wrong package id: {:?}", package_id);
+        }
+    }
+}
+
+/*pub struct CanTxHandler {
     interface: &'static str,
     socket: socket::CanSocket,
     thread_id: ThreadId,
@@ -213,6 +239,8 @@ impl CanTxHandler {
     */
 }
 
+ */
+
 pub fn open_socket(interface: &str) -> Result<socket::CanSocket, errors::CanSocketOpenError> {
     let socket = socket::CanSocket::open(&interface);
     return socket;

src/can_ids.rs

@@ -95,29 +95,29 @@ impl TryFrom<u16> for DeviceId {
 
     fn try_from(v: u16) -> Result<Self, Self::Error> {
         match v {
-            x if x==DeviceId::OBC as u16 => Ok(DeviceId::OBC),
-            x if x==DeviceId::PCDU as u16 => Ok(DeviceId::PCDU),
-            x if x==DeviceId::MGM1 as u16 => Ok(DeviceId::MGM1),
-            x if x==DeviceId::MGM2 as u16 => Ok(DeviceId::MGM2),
-            x if x==DeviceId::MGM3 as u16 => Ok(DeviceId::MGM3),
-            x if x==DeviceId::MGM4 as u16 => Ok(DeviceId::MGM4),
-            x if x==DeviceId::SunSensor1 as u16 => Ok(DeviceId::SunSensor1),
-            x if x==DeviceId::SunSensor2 as u16 => Ok(DeviceId::SunSensor2),
-            x if x==DeviceId::SunSensor3 as u16 => Ok(DeviceId::SunSensor3),
-            x if x==DeviceId::SunSensor4 as u16 => Ok(DeviceId::SunSensor4),
-            x if x==DeviceId::SunSensor5 as u16 => Ok(DeviceId::SunSensor5),
-            x if x==DeviceId::SunSensor6 as u16 => Ok(DeviceId::SunSensor6),
-            x if x==DeviceId::StarTracker as u16 => Ok(DeviceId::StarTracker),
-            x if x==DeviceId::MGT1 as u16 => Ok(DeviceId::MGT1),
-            x if x==DeviceId::MGT2 as u16 => Ok(DeviceId::MGT2),
-            x if x==DeviceId::MGT3 as u16 => Ok(DeviceId::MGT3),
-            x if x==DeviceId::MGT4 as u16 => Ok(DeviceId::MGT4),
-            x if x==DeviceId::RWL1 as u16 => Ok(DeviceId::RWL1),
-            x if x==DeviceId::RWL2 as u16 => Ok(DeviceId::RWL2),
-            x if x==DeviceId::RWL3 as u16 => Ok(DeviceId::RWL3),
-            x if x==DeviceId::RWL4 as u16 => Ok(DeviceId::RWL4),
-            x if x==DeviceId::Camera as u16 => Ok(DeviceId::Camera),
-            x if x==DeviceId::All as u16 => Ok(DeviceId::All),
+            x if x == DeviceId::OBC as u16 => Ok(DeviceId::OBC),
+            x if x == DeviceId::PCDU as u16 => Ok(DeviceId::PCDU),
+            x if x == DeviceId::MGM1 as u16 => Ok(DeviceId::MGM1),
+            x if x == DeviceId::MGM2 as u16 => Ok(DeviceId::MGM2),
+            x if x == DeviceId::MGM3 as u16 => Ok(DeviceId::MGM3),
+            x if x == DeviceId::MGM4 as u16 => Ok(DeviceId::MGM4),
+            x if x == DeviceId::SunSensor1 as u16 => Ok(DeviceId::SunSensor1),
+            x if x == DeviceId::SunSensor2 as u16 => Ok(DeviceId::SunSensor2),
+            x if x == DeviceId::SunSensor3 as u16 => Ok(DeviceId::SunSensor3),
+            x if x == DeviceId::SunSensor4 as u16 => Ok(DeviceId::SunSensor4),
+            x if x == DeviceId::SunSensor5 as u16 => Ok(DeviceId::SunSensor5),
+            x if x == DeviceId::SunSensor6 as u16 => Ok(DeviceId::SunSensor6),
+            x if x == DeviceId::StarTracker as u16 => Ok(DeviceId::StarTracker),
+            x if x == DeviceId::MGT1 as u16 => Ok(DeviceId::MGT1),
+            x if x == DeviceId::MGT2 as u16 => Ok(DeviceId::MGT2),
+            x if x == DeviceId::MGT3 as u16 => Ok(DeviceId::MGT3),
+            x if x == DeviceId::MGT4 as u16 => Ok(DeviceId::MGT4),
+            x if x == DeviceId::RWL1 as u16 => Ok(DeviceId::RWL1),
+            x if x == DeviceId::RWL2 as u16 => Ok(DeviceId::RWL2),
+            x if x == DeviceId::RWL3 as u16 => Ok(DeviceId::RWL3),
+            x if x == DeviceId::RWL4 as u16 => Ok(DeviceId::RWL4),
+            x if x == DeviceId::Camera as u16 => Ok(DeviceId::Camera),
+            x if x == DeviceId::All as u16 => Ok(DeviceId::All),
             _ => Err(()),
         }
     }

src/ccsds.rs

@@ -1,3 +1,4 @@
+use log::warn;
 use crate::tmtc::{MpscStoreAndSendError, PusTcSource, PUS_APID};
 use satrs_core::spacepackets::{CcsdsPacket, SpHeader};
 use satrs_core::tmtc::{CcsdsPacketHandler, ReceivesCcsdsTc};
@@ -29,7 +30,7 @@ impl CcsdsPacketHandler for CcsdsReceiver {
         sp_header: &SpHeader,
         _tc_raw: &[u8],
     ) -> Result<(), Self::Error> {
-        println!("Unknown APID 0x{:x?} detected", sp_header.apid());
+        warn!("Unknown APID 0x{:x?} detected", sp_header.apid());
         Ok(())
     }
 }

src/hk.rs

@@ -1,10 +1,30 @@
+use crate::aocs_handler::{MGMData};
+use crate::requests::Request;
+use crate::requests::RequestWithToken;
+use crate::tmtc::TmStore;
+use eurosim_obsw::{hk_err};
+use satrs_core::pool::StoreAddr;
+use satrs_core::pus::hk::Subservice;
+use satrs_core::pus::verification::{
+    FailParams, StdVerifSenderError, VerificationReporterWithSender,
+};
+use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
+use satrs_core::spacepackets::time::cds::TimeProvider;
+use satrs_core::spacepackets::time::TimeWriter;
+use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
+use satrs_core::spacepackets::SpHeader;
 use satrs_core::tmtc::AddressableId;
+use serde::{Deserialize, Serialize};
+use std::ops::{Deref};
+use std::sync::mpsc::{Receiver, Sender};
+use std::sync::{Arc, Mutex};
 
 pub type CollectionIntervalFactor = u32;
 
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
-pub enum AcsHkIds {
-    TestMgmSet = 1,
+pub enum AocsHkIds {
+    TestAocsSet = 1,
+    TestMgmSet = 2,
 }
 
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
@@ -14,3 +34,154 @@ pub enum HkRequest {
     Disable(AddressableId),
     ModifyCollectionInterval(AddressableId, CollectionIntervalFactor),
 }
+
+#[derive(Serialize, Deserialize)]
+pub struct AocsSensorData {
+    mgm_data: MGMData,  // Voltage for 3 axis
+    css_data: [f64; 18], // Voltage for 18 sun sensors
+    str_data: [f64; 4],  // Quaternion for position of satellite
+}
+
+impl AocsSensorData {
+    pub fn new() -> AocsSensorData {
+        let mgm_data = MGMData::default();
+        let css_data = [0.0; 18];
+        let str_data = [0.0; 4];
+        AocsSensorData {
+            mgm_data,
+            css_data,
+            str_data,
+        }
+    }
+
+    pub fn update_mgm_data(&mut self, mgm_data: &Arc<Mutex<MGMData>>) {
+        let data = mgm_data.lock().unwrap();
+        self.mgm_data = *data;
+    }
+
+    pub fn write_css_data(&mut self, css_data: [f64; 18]) {
+        self.css_data = css_data;
+    }
+
+    pub fn write_str_data(&mut self, str_data: [f64; 4]) {
+        self.str_data = str_data;
+    }
+
+    pub fn get_mgm_data(&mut self) -> MGMData {
+        self.mgm_data
+    }
+
+    pub fn read_css_data(&mut self) -> [f64; 18] {
+        self.css_data
+    }
+
+    pub fn read_str_data(&mut self) -> [f64; 4] {
+        self.str_data
+    }
+}
+
+pub struct AocsHousekeeper {
+    sensor_data_pool: Arc<Mutex<AocsSensorData>>,
+    action_rx: Receiver<RequestWithToken>,
+    seq_count_provider: SeqCountProviderSyncClonable,
+    aocs_tm_store: TmStore,
+    aocs_tm_funnel_tx: Sender<StoreAddr>,
+    verif_reporter: VerificationReporterWithSender<StdVerifSenderError>,
+}
+
+impl AocsHousekeeper {
+    pub fn new(
+        sensor_data_pool: Arc<Mutex<AocsSensorData>>,
+        action_rx: Receiver<RequestWithToken>,
+        seq_count_provider: SeqCountProviderSyncClonable,
+        aocs_tm_store: TmStore,
+        aocs_tm_funnel_tx: Sender<StoreAddr>,
+        verif_reporter: VerificationReporterWithSender<StdVerifSenderError>,
+    ) -> AocsHousekeeper {
+        AocsHousekeeper {
+            sensor_data_pool,
+            action_rx,
+            seq_count_provider,
+            aocs_tm_store,
+            aocs_tm_funnel_tx,
+            verif_reporter,
+        }
+    }
+
+    pub fn handle_hk_request(&mut self) {
+        let mut time_stamp_buf: [u8; 7] = [0; 7];
+
+        if let Ok(request_with_token) = self.action_rx.try_recv() {
+            if let Request::HkRequest(hk_req) = request_with_token.0 {
+                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
+                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
+                let start_token = self //implement this for verification
+                    .verif_reporter
+                    .start_success(request_with_token.1, Some(&time_stamp_buf))
+                    .expect("Error sending start success");
+                if let Ok(()) = match hk_req {
+                    HkRequest::OneShot(id) => self.one_shot_hk(id),
+                    HkRequest::Enable(id) => self.enable_hk(id),
+                    HkRequest::Disable(id) => self.disable_hk(id),
+                    HkRequest::ModifyCollectionInterval(_id, _collection_interval) => Ok(()),
+                } {
+                    let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
+                    cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
+                    self.verif_reporter
+                        .completion_success(start_token, Some(&time_stamp_buf))
+                        .expect("Error sending completion success");
+                } else {
+                    let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
+                    cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
+                    self.verif_reporter
+                        .completion_failure(
+                            start_token,
+                            FailParams::new(
+                                Some(&time_stamp_buf),
+                                &hk_err::UNKNOWN_TARGET_ID,
+                                None,
+                            ),
+                        )
+                        .expect("Error sending completion success");
+                }
+            }
+        }
+    }
+
+    pub fn one_shot_hk(&mut self, id: AddressableId) -> Result<(), ()> {
+        let json_string = self.aocs_data_to_str();
+        self.send_hk_packet(id, &json_string);
+        Ok(())
+    }
+
+    pub fn enable_hk(&mut self, _id: AddressableId) -> Result<(), ()> {
+        Ok(())
+    }
+
+    pub fn disable_hk(&mut self, _id: AddressableId) -> Result<(), ()> {
+        Ok(())
+    }
+
+    pub fn aocs_data_to_str(&mut self) -> String {
+        let pool = self.sensor_data_pool.lock().unwrap();
+        serde_json::to_string(pool.deref()).unwrap()
+    }
+
+    pub fn send_hk_packet(&mut self, id: AddressableId, data: &str) {
+        let mut time_stamp_buf: [u8; 7] = [0; 7];
+        let mut huge_buf: [u8; 8192] = [0; 8192];
+
+        let mut sp_header =
+            SpHeader::tm_unseg(0x02, self.seq_count_provider.get_and_increment(), 0).unwrap();
+        let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
+        cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
+        let mut len = id.write_to_be_bytes(&mut huge_buf).unwrap();
+        huge_buf[8..data.len() + 8].copy_from_slice(data.as_bytes());
+        len += data.len();
+        let tm_sec_header =
+            PusTmSecondaryHeader::new_simple(3, Subservice::TmHkPacket as u8, &time_stamp_buf);
+        let hk_tm = PusTm::new(&mut sp_header, tm_sec_header, Some(&huge_buf[0..len]), true);
+        let addr = self.aocs_tm_store.add_pus_tm(&hk_tm);
+        self.aocs_tm_funnel_tx.send(addr).expect("sending failed");
+    }
+}

src/main.rs

@@ -1,18 +1,24 @@
+#![allow(dead_code)]
+#![allow(unused_variables)]
+// remove this, just here for now since application isn't fully developed
+
 mod action;
 mod aocs;
+mod aocs_handler;
 mod cam;
+#[cfg(feature = "can")]
 mod can;
 mod can_ids;
 mod ccsds;
-mod pld_handler;
 mod hk;
 mod logger;
+mod pld_handler;
+mod power_handler;
 mod pus;
 mod requests;
 mod tmtc;
-mod pcdu;
 
-use crate::requests::{Request, RequestWithToken};
+use crate::requests::{RequestWithToken};
 use crate::tmtc::{
     core_tmtc_task, OtherArgs, PusTcSource, TcArgs, TcStore, TmArgs, TmFunnel, TmStore, PUS_APID,
 };
@@ -23,37 +29,33 @@ use satrs_core::event_man::{
 use satrs_core::events::EventU32;
 use satrs_core::pool::{LocalPool, PoolCfg, StoreAddr};
 use satrs_core::pus::event_man::{
-    DefaultPusMgmtBackendProvider, EventReporter, EventRequestWithToken,
-    PusEventDispatcher,
+    DefaultPusMgmtBackendProvider, EventReporter, EventRequestWithToken, PusEventDispatcher,
 };
 use satrs_core::pus::verification::{
     MpscVerifSender, VerificationReporterCfg, VerificationReporterWithSender,
 };
 use satrs_core::pus::{EcssTmErrorWithSend, EcssTmSenderCore};
-use satrs_core::seq_count::{
-    SeqCountProviderSyncClonable,
-};
-use satrs_core::{
-    spacepackets::time::cds::TimeProvider,
-    spacepackets::time::TimeWriter,
-    spacepackets::tm::{PusTm},
-};
+use satrs_core::seq_count::SeqCountProviderSyncClonable;
+use satrs_core::{spacepackets::tm::PusTm};
 use strum::IntoEnumIterator;
 
-use crate::can_ids::{can_id_to_package_id, DeviceId, load_package_ids, PackageId, PackageModel, ThreadId};
-use embedded_can::{Id, StandardId};
-use log::{info, warn};
+use crate::can_ids::{
+    load_package_ids, DeviceId, PackageModel, ThreadId,
+};
+use log::{info};
+use satrs_core::power::{SwitchId, SwitchState};
 use std::collections::HashMap;
 use std::net::{IpAddr, SocketAddr};
-use std::sync::mpsc::{channel};
-use std::sync::{mpsc, Arc, RwLock, Mutex};
+use std::sync::mpsc::channel;
+use std::sync::{mpsc, Arc, Mutex, RwLock};
 use std::thread;
-use satrs_core::power::{SwitchId, SwitchState};
 //use libc::time64_t;
-use crate::action::ActionRequest;
-use crate::cam::CameraRequest;
-use crate::pld_handler::{CameraHandler, core_pld_task};
-use crate::pcdu::{core_power_task, PowerSwitcher};
+use crate::aocs_handler::{MGMData, MGMHandler};
+#[cfg(feature = "can")]
+use crate::can::CanTxHandler;
+use crate::hk::{AocsHousekeeper, AocsSensorData};
+use crate::pld_handler::{core_pld_task};
+use crate::power_handler::{core_power_task, PowerSwitcher};
 
 #[derive(Clone)]
 struct EventTmSender {
@@ -82,9 +84,8 @@ impl EcssTmSenderCore for EventTmSender {
 }
 
 fn main() {
-    println!("Running SESPSat OBSW");
-
     logger::setup_logger().unwrap();
+    info!("Running DemoSat OBSW!");
 
     let tm_pool = LocalPool::new(PoolCfg::new(vec![
         (30, 32),
@@ -136,19 +137,19 @@ fn main() {
     let mut event_man = EventManagerWithMpscQueue::new(Box::new(event_recv));
     let event_reporter = EventReporter::new(PUS_APID, 128).unwrap();
     let pus_tm_backend = DefaultPusMgmtBackendProvider::<EventU32>::default();
-    let mut pus_event_dispatcher =
+    let pus_event_dispatcher =
         PusEventDispatcher::new(event_reporter, Box::new(pus_tm_backend));
     let (pus_event_man_tx, pus_event_man_rx) = channel();
     let pus_event_man_send_provider = MpscEventU32SendProvider::new(1, pus_event_man_tx);
-    let mut reporter_event_handler = verif_reporter.clone();
-    let mut reporter_aocs = verif_reporter.clone();
+    let reporter_event_handler = verif_reporter.clone();
+    let reporter_aocs = verif_reporter.clone();
     let mut reporter_pld = verif_reporter.clone();
     event_man.subscribe_all(pus_event_man_send_provider.id());
 
     let mut request_map = HashMap::new();
-    let (acs_thread_tx, acs_thread_rx) = channel::<RequestWithToken>();
+    let (aocs_thread_tx, aocs_thread_rx) = channel::<RequestWithToken>();
     let (pld_thread_tx, pld_thread_rx) = channel::<RequestWithToken>();
-    request_map.insert(RequestTargetId::AcsSubsystem as u32, acs_thread_tx);
+    request_map.insert(RequestTargetId::AcsSubsystem as u32, aocs_thread_tx);
     request_map.insert(RequestTargetId::PldSubsystem as u32, pld_thread_tx);
     //add here receivers for tmtc task to send requests to
     //request_map.insert(RequestTargetId::CanTask as u32, can_thread_tx);
@@ -189,154 +190,82 @@ fn main() {
     // get package id hashmap
     let package_ids_rx = load_package_ids();
 
-    let socket0 = can::CanRxHandler::new_socket("can0", can_senders, package_ids_rx).unwrap();
-
     info!("Starting TMTC task");
     let builder0 = thread::Builder::new().name("TMTCThread".into());
     let jh0 = builder0.spawn(move || {
         core_tmtc_task(core_args, tc_args, tm_args);
     });
 
+    let (can_tx_sender, can_tx_receiver) = channel();
+
+    #[cfg(feature = "can")]
+    let mut can_rx_socket = can::CanRxHandler::new_socket("can0", can_senders.clone(), package_ids_rx.clone()).unwrap();
+
+    #[cfg(feature = "can")]
     info!("Starting CAN Socket listening task");
     let builder1 = thread::Builder::new().name("CanRxHandler".into());
     let jh1 = builder1.spawn(move || loop {
-        let frame = socket0.rx_socket();
-        if let Some(frame) = frame {
-            let forward = socket0.forward_frame(frame);
-        }
+        #[cfg(feature = "can")]
+        can_rx_socket.process_incoming();
+    });
+
+    #[cfg(feature = "can")]
+    let mut can_tx_socket = CanTxHandler::new_socket("can0", package_ids_rx.clone(), can_tx_receiver).unwrap();
+
+    #[cfg(feature = "can")]
+    info!("Starting CAN Socket writing task");
+    let builder_can_tx = thread::Builder::new().name("TxHandler".into());
+    let jh_can_tx = builder_can_tx.spawn( move || loop {
+        #[cfg(feature = "can")]
+        can_tx_socket.process_incoming();
     });
 
     let (pcdu_tx, pcdu_rx) = mpsc::channel::<(SwitchId, SwitchState)>();
-    let pcdu_can_tx =
-        can::CanTxHandler::new_socket("can0", ThreadId::PowerThread, load_package_ids()).unwrap();
-
+    let pcdu_can_tx_sender =
+        can_tx_sender.clone();
 
     let mut device_state_map = HashMap::new();
-        for id in DeviceId::iter() {
-            device_state_map.insert(id, SwitchState::Off);
-        }
+    for id in DeviceId::iter() {
+        device_state_map.insert(id, SwitchState::Off);
+    }
     let clonable_device_state_map = Arc::new(Mutex::new(device_state_map));
 
-    let mut power_switcher = PowerSwitcher::new(pcdu_tx, clonable_device_state_map.clone());
+    let power_switcher = PowerSwitcher::new(pcdu_tx, clonable_device_state_map.clone());
 
     info!("Starting power task");
     let builder2 = thread::Builder::new().name("PowerThread".into());
     let jh2 = builder2.spawn(move || {
-        core_power_task(pcdu_rx, pcdu_can_tx, power_can_rx, clonable_device_state_map.clone());
+        core_power_task(
+            pcdu_rx,
+            pcdu_can_tx_sender,
+            power_can_rx,
+            clonable_device_state_map.clone(),
+        );
     });
 
-
-    let package_map_aocs_tx = load_package_ids();
-    let aocs_tm_funnel_tx = tm_funnel_tx.clone();
-    let mut aocs_tm_store = tm_store.clone();
-
-    /*
-    // AOCS Thread
-    let socket1 =
-        can::CanTxHandler::new_socket("can0", ThreadId::AOCSThread, package_map_aocs_tx).unwrap();
-    info!("Starting AOCS receiving thread");
-    let builder2 = thread::Builder::new().name("AOCSThread".into());
-    let jh2 = builder2.spawn(move || {
-        let mut time_stamp_buf: [u8; 7] = [0; 7];
-        let mut huge_buf: [u8; 8192] = [0; 8192];
-        let data: [u8; 3] = [1, 2, 3];
-        let current_mgm_data = MgmData::default();
-        //let current_mgt_data = MgtData::default();
-        //let current_
-        loop {
-            // device handling
-            //info!("Sending {:?}", PackageId::AOCSControlMGT1);
-            //socket1.tx_socket(PackageId::AOCSControlMGT1, &data);
-            //info!("Waiting for {:?}", PackageId::AOCSDataMGM1);
-            let msg = aocs_can_rx.try_recv();
-            //current_mgm_data.x = new_data
-            match aocs_can_rx.try_recv() {
-                Ok(package) => match package.package_id() {
-                    _ => warn!("Incorrect Id"),
-                },
-                Err(_) => {}
-            }
-
-            // telecommand handling
-            match acs_thread_rx.try_recv() {
-                Ok(request_with_token) => {
-                    match request_with_token.0 {
-                        Request::HkRequest(hk_req) => {
-                            match hk_req {
-                                HkRequest::OneShot(id) => {
-                                    assert_eq!(id.target_id, RequestTargetId::AcsSubsystem as u32);
-                                    if id.unique_id == 0 {
-                                        let mut sp_header = SpHeader::tm_unseg(
-                                            0x02,
-                                            aocs_seq_count_provider.get_and_increment(),
-                                            0,
-                                        )
-                                        .unwrap();
-                                        let cds_stamp =
-                                            TimeProvider::from_now_with_u16_days().unwrap();
-                                        cds_stamp.write_to_bytes(&mut time_stamp_buf);
-                                        let mut len = id.write_to_be_bytes(&mut huge_buf).unwrap();
-                                        let json_string = "asdf";
-                                        huge_buf[8..json_string.len() + 8]
-                                            .copy_from_slice(json_string.as_bytes());
-                                        len += json_string.len();
-                                        let tm_sec_header = PusTmSecondaryHeader::new_simple(
-                                            3,
-                                            Subservice::TmHkPacket as u8,
-                                            &time_stamp_buf,
-                                        );
-                                        let hk_tm = PusTm::new(
-                                            &mut sp_header,
-                                            tm_sec_header,
-                                            Some(&huge_buf[0..len]),
-                                            true,
-                                        );
-                                        let addr = aocs_tm_store.add_pus_tm(&hk_tm);
-                                        aocs_tm_funnel_tx.send(addr).expect("sending failed");
-                                        /*            let start_token = self    //implement this for verification
-                                            .verif_reporter
-                                            .start_success(token, &self.time_stamp)
-                                            .expect("Error sending start success");
-                                        self.tm_tx
-                                            .send(addr)
-                                            .expect("Sending TM to TM funnel failed");
-                                        self.verif_reporter
-                                            .completion_success(start_token, &self.time_stamp)
-                                            .expect("Error sending completion success");
-
-                                                                     */
-                                    }
-                                }
-                                HkRequest::Enable(_) => {}
-                                HkRequest::Disable(_) => {}
-                                HkRequest::ModifyCollectionInterval(_, _) => {}
-                            }
-                        }
-                        _ => {}
-                    }
-                }
-                Err(_) => {}
-            }
-        }
-    });
-
-     */
-
     let package_map_pld_tx = load_package_ids();
     let pld_tm_funnel_tx = tm_funnel_tx.clone();
-    let mut pld_tm_store = tm_store.clone();
+    let pld_tm_store = tm_store.clone();
 
-    let PLDCanSocket =
-        can::CanTxHandler::new_socket("can0", ThreadId::PLDThread, package_map_pld_tx).unwrap();
+    let pld_can_tx_sender =
+        can_tx_sender.clone();
+
+    let power_switcher_pld = power_switcher.clone();
 
     //let mut pcdu_tx_clone = pcdu_tx.clone();
-    println!("Starting Payload Handling task");
+    info!("Starting Payload Handling task");
     let builder3 = thread::Builder::new().name("PLDThread".into());
     let jh3 = builder3.spawn(move || {
-        core_pld_task(power_switcher.clone(), pld_thread_rx, pld_can_rx, &mut reporter_pld);
+        core_pld_task(
+            power_switcher_pld.clone(),
+            pld_thread_rx,
+            pld_can_rx,
+            pld_can_tx_sender,
+            &mut reporter_pld,
+        );
     });
 
-    println!("Starting TM funnel task");
+    info!("Starting TM funnel task");
     let builder4 = thread::Builder::new().name("TMFunnelThread".into());
     let jh4 = builder4.spawn(move || {
         let tm_funnel = TmFunnel {
@@ -353,15 +282,58 @@ fn main() {
         }
     });
 
+    let package_map_aocs_tx = load_package_ids();
+    let aocs_tm_funnel_tx = tm_funnel_tx.clone();
+    let aocs_tm_store = tm_store.clone();
+
+    let mgm_shared_data: Arc<Mutex<MGMData>> = Arc::default();
+
+    let aocs_sensor_data = Arc::new(Mutex::new(AocsSensorData::new()));
+    let (aocs_mode_tx, aocs_mode_rx) = channel();
+    let (mgm_action_tx, mgm_action_rx) = channel();
+
+    let power_switcher_aocs = power_switcher.clone();
+
+    info!("Starting AOCS task");
+    let builder5 = thread::Builder::new().name("AOCSThread".into());
+    let jh5 = builder5.spawn(move || {
+        let mut mgm_handler = MGMHandler::new(power_switcher_aocs.clone(), DeviceId::MGM1, can_tx_sender.clone(), aocs_can_rx, aocs_mode_rx, mgm_action_rx);
+
+        let aocs_sensor_data = Arc::new(Mutex::new(AocsSensorData::new()));
+        let mut aocs_housekeeper = AocsHousekeeper::new(
+            aocs_sensor_data.clone(),
+            aocs_thread_rx,
+            aocs_seq_count_provider,
+            aocs_tm_store,
+            aocs_tm_funnel_tx,
+            reporter_aocs,
+        );
+        loop {
+            mgm_handler.periodic_op();
+            let mut locked_sensor_data = aocs_sensor_data.lock().unwrap();
+            locked_sensor_data.update_mgm_data(&mgm_handler.get_data_ref());
+            drop(locked_sensor_data);
+            aocs_housekeeper.handle_hk_request();
+        }
+    });
+
     jh0.unwrap()
         .join()
         .expect("Joining UDP TMTC server thread failed");
+
+
     jh1.unwrap()
         .join()
         .expect("Joining CAN Bus Listening thread failed");
+    jh_can_tx.unwrap()
+        .join()
+        .expect("Joining CAN Bus Writing thread failed");
     jh2.unwrap().join().expect("Joining power thread failed");
     jh3.unwrap().join().expect("Joining PLD thread failed");
-    jh4.unwrap().join().expect("Joining TM funnel thread failed");
+    jh4.unwrap()
+        .join()
+        .expect("Joining TM funnel thread failed");
+    jh5.unwrap().join().expect("Joining AOCS thread failed");
 }
 #[derive(Default)]
 struct MgmData {

src/pld_handler.rs

@@ -1,17 +1,15 @@
-use std::sync::mpsc;
-use std::sync::mpsc::{Receiver, Sender, TryRecvError};
-use log::info;
-use satrs_core::power::{PowerSwitcherCommandSender, PowerSwitchInfo, SwitchId, SwitchState};
+use crate::action::ActionRequest;
+use crate::can_ids::{DeviceId, PackageId, PackageModel};
+use crate::power_handler::{DeviceState, PowerSwitcher};
+use crate::requests::{Request, RequestWithToken};
+use eurosim_obsw::RequestTargetId;
+use log::{debug};
+use satrs_core::power::{PowerSwitchInfo, PowerSwitcherCommandSender, SwitchId};
 use satrs_core::pus::verification::{StdVerifSenderError, VerificationReporterWithSender};
 use satrs_core::spacepackets::time::cds::TimeProvider;
 use satrs_core::spacepackets::time::TimeWriter;
-use eurosim_obsw::RequestTargetId;
-use crate::action::ActionRequest;
-use crate::can::CanTxHandler;
-use crate::can_ids::{DeviceId, load_package_ids, PackageId, PackageModel, ThreadId};
-use crate::pld_handler::CameraMode::PictureRequest;
-use crate::pcdu::{DeviceState, PowerSwitcher};
-use crate::requests::{Request, RequestWithToken};
+use std::sync::mpsc;
+use std::sync::mpsc::{Receiver, Sender};
 
 #[derive(Debug, PartialEq, Copy, Clone)]
 pub enum CameraMode {
@@ -27,22 +25,40 @@ pub struct CameraHandler {
     camera_device_id: DeviceId,
     camera_switch_id: SwitchId,
     device_state: DeviceState,
-    can_tx: CanTxHandler,
+    can_tx: Sender<PackageModel>,
     can_rx: Receiver<PackageModel>,
     mode: CameraMode,
     mode_rx: Receiver<CameraMode>,
+    action_rx: Receiver<RequestWithToken>,
 }
 
 impl CameraHandler {
-    pub fn new(power_switcher: PowerSwitcher, camera_device_id: DeviceId, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>, mode_rx: Receiver<CameraMode>) -> CameraHandler {
+    pub fn new(
+        power_switcher: PowerSwitcher,
+        camera_device_id: DeviceId,
+        can_tx: Sender<PackageModel>,
+        can_rx: Receiver<PackageModel>,
+        mode_rx: Receiver<CameraMode>,
+        action_rx: Receiver<RequestWithToken>,
+    ) -> CameraHandler {
         let camera_switch_id = camera_device_id as u16;
-        CameraHandler{power_switcher, camera_device_id, camera_switch_id, device_state: DeviceState::Off, can_tx, can_rx, mode: CameraMode::Idle, mode_rx}
+        CameraHandler {
+            power_switcher,
+            camera_device_id,
+            camera_switch_id,
+            device_state: DeviceState::Off,
+            can_tx,
+            can_rx,
+            mode: CameraMode::Idle,
+            mode_rx,
+            action_rx,
+        }
     }
 
     pub fn set_mode(&mut self, mode: CameraMode) {
-        if self.mode == CameraMode::Idle{
+        if self.mode == CameraMode::Idle {
             if mode == CameraMode::PictureRequest {
-                self.mode = PictureRequest;
+                self.mode = CameraMode::PictureRequest;
             }
         }
     }
@@ -51,84 +67,17 @@ impl CameraHandler {
         self.mode
     }
 
-    pub fn periodic_op(&mut self) {
-        // Camera Device Handler State Machine
-
-        match self.mode {
-            CameraMode::Idle => {}
-            CameraMode::PictureRequest => {
-                if self.device_state == DeviceState::Off {
-                    self.power_switcher.send_switch_on_cmd(self.camera_switch_id).expect("sending switch cmd failed");
-                    self.device_state = DeviceState::SwitchingPower;
-                    info!("switching power");
-                }
-                if self.device_state == DeviceState::SwitchingPower {
-                    if self.power_switcher.get_is_switch_on(self.camera_switch_id).expect("reading switch state failed") {
-                        self.device_state = DeviceState::On;
-                        info!("device on");
-                    }
-                }
-                if self.device_state == DeviceState::On {
-                    self.can_tx.tx_socket(PackageId::CameraImageRequest, &[1]);
-                    info!("sent camera request");
-                    self.mode = CameraMode::Verification;
-                }
-            }
-            CameraMode::Verification => {
-                if self.device_state == DeviceState::On {
-                    info!("waiting for image request confirmation");
-                    if let Ok(msg) = self.can_rx.recv() {
-                        if msg.package_id() == PackageId::CameraImageRequestConfirmation {
-                            self.mode = CameraMode::Start;
-                        }
-                    }
-                }
-            }
-            CameraMode::Start => {
-                if self.device_state == DeviceState::On {
-                    info!("waiting for image start confirmation");
-                    if let Ok(msg) = self.can_rx.recv() {
-                        if msg.package_id() == PackageId::CameraImageExecutionStart {
-                            self.mode = CameraMode::End;
-                        }
-                    }
-                }
-            }
-            CameraMode::End => {
-                if self.device_state == DeviceState::On {
-                    info!("waiting for image end confirmation");
-                    if let Ok(msg) = self.can_rx.recv() {
-                        if msg.package_id() == PackageId::CameraImageExectutionEnd {
-                            self.power_switcher.send_switch_off_cmd(self.camera_switch_id).expect("sending switch command failed");
-                            self.device_state = DeviceState::SwitchingPower;
-                            info!("switching power");
-                        }
-                    }
-                }
-                if self.device_state == DeviceState::SwitchingPower {
-                    if !self.power_switcher.get_is_switch_on(self.camera_switch_id).expect("reading switch state failed") {
-                        self.device_state = DeviceState::Off;
-                        info!("device off");
-                    }
-                }
-                if self.device_state == DeviceState::Off {
-                    self.mode = CameraMode::Idle;
-                }
+    pub fn handle_mode_requests(&mut self) {
+        match self.mode_rx.try_recv() {
+            Ok(mode) => {
+                self.set_mode(mode);
             }
+            Err(_) => {}
         }
     }
-}
 
-pub fn core_pld_task(power_switcher: PowerSwitcher, pld_thread_rx: Receiver<RequestWithToken>, pld_can_rx: Receiver<PackageModel>, reporter_pld: &mut VerificationReporterWithSender<StdVerifSenderError>) {
-    let (camera_mode_tx, camera_mode_rx) = mpsc::channel();
-    let camera_can_tx =
-        CanTxHandler::new_socket("can0", ThreadId::PLDThread, load_package_ids()).unwrap();
-
-    let mut camera_handler = CameraHandler::new(power_switcher, DeviceId::Camera, camera_can_tx, pld_can_rx, camera_mode_rx);
-
-    let mut time_stamp_buf: [u8; 7] = [0; 7];
-    loop {
-        match pld_thread_rx.try_recv() {
+    /*pub fn handle_action_requests(&mut self) {
+        match self.action_rx.try_recv() {
             Ok(request_with_token) => {
                 match request_with_token.0 {
                     Request::ActionRequest(action_id) => {
@@ -144,11 +93,13 @@ pub fn core_pld_task(power_switcher: PowerSwitcher, pld_thread_rx: Receiver<Requ
                                     .start_success(request_with_token.1, Some(&time_stamp_buf))
                                     .expect("Error sending start success.");
 
-                                info!("{:?}", camera_handler.get_mode());
+                                debug!("{:?}", camera_handler.get_mode());
                                 while camera_handler.get_mode() != CameraMode::Idle {
                                     camera_handler.periodic_op();
                                 }
 
+                                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
+                                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
                                 // send end verification with token
                                 reporter_pld
                                     .completion_success(
@@ -168,4 +119,147 @@ pub fn core_pld_task(power_switcher: PowerSwitcher, pld_thread_rx: Receiver<Requ
         }
     }
 
-}
+     */
+
+    pub fn take_picture(&mut self) {}
+
+    pub fn periodic_op(&mut self) {
+        // Camera Device Handler State Machine
+
+        match self.mode {
+            CameraMode::Idle => {}
+            CameraMode::PictureRequest => {
+                if self.device_state == DeviceState::Off {
+                    self.power_switcher
+                        .send_switch_on_cmd(self.camera_switch_id)
+                        .expect("sending switch cmd failed");
+                    self.device_state = DeviceState::SwitchingPower;
+                    debug!("switching power");
+                }
+                if self.device_state == DeviceState::SwitchingPower {
+                    if self
+                        .power_switcher
+                        .get_is_switch_on(self.camera_switch_id)
+                        .expect("reading switch state failed")
+                    {
+                        self.device_state = DeviceState::On;
+                        debug!("device on");
+                    }
+                }
+                if self.device_state == DeviceState::On {
+                    self.can_tx.send(PackageModel::new(PackageId::CameraImageRequest, &[1]).unwrap()).unwrap();
+                    debug!("sent camera request");
+                    self.mode = CameraMode::Verification;
+                }
+            }
+            CameraMode::Verification => {
+                if self.device_state == DeviceState::On {
+                    debug!("waiting for image request confirmation");
+                    if let Ok(msg) = self.can_rx.recv() {
+                        if msg.package_id() == PackageId::CameraImageRequestConfirmation {
+                            self.mode = CameraMode::Start;
+                        }
+                    }
+                }
+            }
+            CameraMode::Start => {
+                if self.device_state == DeviceState::On {
+                    debug!("waiting for image start confirmation");
+                    if let Ok(msg) = self.can_rx.recv() {
+                        if msg.package_id() == PackageId::CameraImageExecutionStart {
+                            self.mode = CameraMode::End;
+                        }
+                    }
+                }
+            }
+            CameraMode::End => {
+                if self.device_state == DeviceState::On {
+                    debug!("waiting for image end confirmation");
+                    if let Ok(msg) = self.can_rx.recv() {
+                        if msg.package_id() == PackageId::CameraImageExectutionEnd {
+                            self.power_switcher
+                                .send_switch_off_cmd(self.camera_switch_id)
+                                .expect("sending switch command failed");
+                            self.device_state = DeviceState::SwitchingPower;
+                            debug!("switching power");
+                        }
+                    }
+                }
+                if self.device_state == DeviceState::SwitchingPower {
+                    if !self
+                        .power_switcher
+                        .get_is_switch_on(self.camera_switch_id)
+                        .expect("reading switch state failed")
+                    {
+                        self.device_state = DeviceState::Off;
+                        debug!("device off");
+                    }
+                }
+                if self.device_state == DeviceState::Off {
+                    self.mode = CameraMode::Idle;
+                }
+            }
+        }
+    }
+}
+
+pub fn core_pld_task(
+    power_switcher: PowerSwitcher,
+    pld_thread_rx: Receiver<RequestWithToken>,
+    pld_can_rx: Receiver<PackageModel>,
+    pld_can_tx: Sender<PackageModel>,
+    reporter_pld: &mut VerificationReporterWithSender<StdVerifSenderError>,
+) {
+    let (_camera_mode_tx, camera_mode_rx) = mpsc::channel();
+    let (_action_tx, action_rx) = mpsc::channel();
+
+    let mut camera_handler = CameraHandler::new(
+        power_switcher,
+        DeviceId::Camera,
+        pld_can_tx,
+        pld_can_rx,
+        camera_mode_rx,
+        action_rx,
+    );
+
+    let mut time_stamp_buf: [u8; 7] = [0; 7];
+    loop {
+        match pld_thread_rx.try_recv() {
+            Ok(request_with_token) => {
+                match request_with_token.0 {
+                    Request::ActionRequest(action_id) => {
+                        match action_id {
+                            ActionRequest::ImageRequest(target_id) => {
+                                assert_eq!(target_id, RequestTargetId::PldSubsystem);
+                                camera_handler.set_mode(CameraMode::PictureRequest);
+
+                                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
+                                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
+                                // send start verification and get token
+                                let start_token = reporter_pld
+                                    .start_success(request_with_token.1, Some(&time_stamp_buf))
+                                    .expect("Error sending start success.");
+
+                                debug!("{:?}", camera_handler.get_mode());
+                                while camera_handler.get_mode() != CameraMode::Idle {
+                                    camera_handler.periodic_op();
+                                }
+
+                                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
+                                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
+                                // send end verification with token
+                                reporter_pld
+                                    .completion_success(start_token, Some(&time_stamp_buf))
+                                    .expect("Error sending start success.");
+                            }
+                            ActionRequest::OrientationRequest(_) => {}
+                            ActionRequest::PointingRequest(_) => {}
+                        }
+                    }
+                    _ => {}
+                }
+            }
+            Err(_) => {}
+        }
+    }
+}

src/power_handler.rs

@@ -1,17 +1,18 @@
-use std::collections::HashMap;
-use std::sync::mpsc::{Receiver, Sender};
-use satrs_core::power::{PowerSwitcherCommandSender, PowerSwitchInfo, PowerSwitchProvider, SwitchId, SwitchState};
-use crate::can::{CanTxHandler};
 use crate::can_ids::{DeviceId, PackageId, PackageModel};
+use log::{debug};
+use satrs_core::power::{
+    PowerSwitchInfo, PowerSwitchProvider, PowerSwitcherCommandSender, SwitchId, SwitchState,
+};
+use std::collections::HashMap;
 use std::convert::TryFrom;
+use std::sync::mpsc::{Receiver, Sender};
 use std::sync::{Arc, Mutex};
 use std::time::Duration;
 use std::vec;
-use log::info;
 pub use strum::IntoEnumIterator; // 0.17.1
 pub use strum_macros::EnumIter; // 0.17.1
 
-#[derive(Copy, Clone, Debug, PartialEq,  EnumIter)]
+#[derive(Copy, Clone, Debug, PartialEq, EnumIter)]
 pub enum DeviceState {
     On,
     Off,
@@ -28,14 +29,20 @@ pub struct PowerSwitcher {
 
 pub struct PCDU {
     switch_rx: Receiver<(SwitchId, SwitchState)>,
-    can_tx: CanTxHandler,
+    can_tx: Sender<PackageModel>,
     can_rx: Receiver<PackageModel>,
     device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>,
 }
 
 impl PowerSwitcher {
-    pub fn new(switch_tx: Sender<(SwitchId, SwitchState)>, device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>) -> PowerSwitcher {
-        PowerSwitcher{switch_tx, device_state_map}
+    pub fn new(
+        switch_tx: Sender<(SwitchId, SwitchState)>,
+        device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>,
+    ) -> PowerSwitcher {
+        PowerSwitcher {
+            switch_tx,
+            device_state_map,
+        }
     }
 }
 
@@ -44,16 +51,16 @@ impl PowerSwitcherCommandSender for PowerSwitcher {
 
     fn send_switch_on_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error> {
         return match self.switch_tx.send((switch_id, SwitchState::On)) {
-            Ok(_) => {Ok(())}
-            Err(_) => {Err(())}
-        }
+            Ok(_) => Ok(()),
+            Err(_) => Err(()),
+        };
     }
 
     fn send_switch_off_cmd(&mut self, switch_id: SwitchId) -> Result<(), Self::Error> {
         return match self.switch_tx.send((switch_id, SwitchState::Off)) {
-            Ok(_) => {Ok(())}
-            Err(_) => {Err(())}
-        }
+            Ok(_) => Ok(()),
+            Err(_) => Err(()),
+        };
     }
 }
 
@@ -67,11 +74,10 @@ impl PowerSwitchInfo for PowerSwitcher {
                 Ok(*state)
             } else {
                 Err(())
-            }
+            };
         } else {
             Err(())
         }
-
     }
 
     fn switch_delay_ms(&self) -> u32 {
@@ -84,81 +90,85 @@ impl PowerSwitchProvider for PowerSwitcher {
 }
 
 impl PCDU {
-    pub fn new(switch_rx: Receiver<(SwitchId, SwitchState)>, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>, device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>) -> PCDU{
-        PCDU{switch_rx, can_tx, can_rx, device_state_map}
+    pub fn new(
+        switch_rx: Receiver<(SwitchId, SwitchState)>,
+        can_tx: Sender<PackageModel>,
+        can_rx: Receiver<PackageModel>,
+        device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>,
+    ) -> PCDU {
+        PCDU {
+            switch_rx,
+            can_tx,
+            can_rx,
+            device_state_map,
+        }
     }
 
     pub fn send_power_on(&mut self, switch_id: SwitchId) -> Result<(), ()> {
         return if let Ok(dev_id) = DeviceId::try_from(switch_id) {
             let dev_id_bytes = dev_id as u8;
             let buf: &[u8] = &dev_id_bytes.to_be_bytes();
-            self.can_tx.tx_socket(PackageId::DevicePowerOnRequest, buf);
+            self.can_tx.send(PackageModel::new(PackageId::DevicePowerOnRequest, buf).unwrap()).unwrap();
             let mut map_lock = self.device_state_map.lock().unwrap();
             *map_lock.get_mut(&dev_id).unwrap() = SwitchState::Unknown;
-            self.can_rx.recv();
-            self.can_rx.recv();
+            // TODO: potentially change bus logic -> remove acceptance and verification of power off/on, since status is simply called in next step anyway
+            self.can_rx.recv().unwrap();
+            self.can_rx.recv().unwrap();
             Ok(())
         } else {
             Err(())
-        }
+        };
     }
 
     pub fn send_power_off(&mut self, switch_id: SwitchId) -> Result<(), ()> {
         return if let Ok(dev_id) = DeviceId::try_from(switch_id) {
             let dev_id_bytes = dev_id as u8;
             let buf: &[u8] = &dev_id_bytes.to_be_bytes();
-            self.can_tx.tx_socket(PackageId::DevicePowerOffRequest, buf);
+            self.can_tx.send(PackageModel::new(PackageId::DevicePowerOffRequest, buf).unwrap()).unwrap();
             let mut map_lock = self.device_state_map.lock().unwrap();
             *map_lock.get_mut(&dev_id).unwrap() = SwitchState::Unknown;
-            self.can_rx.recv();
-            self.can_rx.recv();
+            self.can_rx.recv().unwrap();
+            self.can_rx.recv().unwrap();
             Ok(())
         } else {
             Err(())
-        }
+        };
     }
 
     pub fn update_states_helper(&mut self, dev_id: &DeviceId) -> Result<(), ()> {
-        let switch_id: SwitchId = *dev_id as u16;
+        let _switch_id: SwitchId = *dev_id as u16;
         let dev_id_bytes = *dev_id as u8;
         let buf: &[u8] = &dev_id_bytes.to_be_bytes();
-        self.can_tx.tx_socket(PackageId::DevicePowerStatusRequest, buf);
+        self.can_tx.send(PackageModel::new(PackageId::DevicePowerStatusRequest, buf).unwrap()).unwrap();
         match self.can_rx.recv_timeout(Duration::from_secs(10)) {
             Ok(msg) => {
-                if msg.package_id() == PackageId::DevicePowerStatusResponse && msg.data()[0] == dev_id_bytes{
-                    info!("received power status response");
+                if msg.package_id() == PackageId::DevicePowerStatusResponse
+                    && msg.data()[0] == dev_id_bytes
+                {
+                    debug!("received power status response");
                     let mut map_lock = self.device_state_map.lock().unwrap();
-                    let mut state: SwitchState;
+                    let state: SwitchState;
                     match msg.data()[1] {
-                        0 => {
-                            state = SwitchState::Off
-                        },
-                        1 => {
-                            state = SwitchState::On
-                        },
-                        2 => {
-                            state = SwitchState::Faulty
-                        },
+                        0 => state = SwitchState::Off,
+                        1 => state = SwitchState::On,
+                        2 => state = SwitchState::Faulty,
                         _ => {
                             return Err(());
                         }
                     }
                     *map_lock.get_mut(&dev_id).unwrap() = state;
-                    info!("{:?}", map_lock);
+                    debug!("{:?}", map_lock);
                     Ok(())
                 } else {
                     Err(())
                 }
             }
-            Err(_) => {
-                Err(())
-            }
+            Err(_) => Err(()),
         }
-
     }
 
     pub fn update_all_states_helper(&mut self) -> Result<(), ()> {
-        let mut map_lock = self.device_state_map.lock().unwrap();
+        let map_lock = self.device_state_map.lock().unwrap();
         let mut device_list = vec::Vec::new();
         for key in map_lock.keys() {
             device_list.push(key.clone());
@@ -173,53 +183,50 @@ impl PCDU {
     pub fn update_switch_states(&mut self, switch_id: SwitchId) -> Result<(), ()> {
         return if let Ok(dev_id) = DeviceId::try_from(switch_id) {
             match dev_id {
-                DeviceId::All => {
-                    self.update_all_states_helper()
-                }
-                _ => {
-                    self.update_states_helper(&dev_id)
-                }
+                DeviceId::All => self.update_all_states_helper(),
+                _ => self.update_states_helper(&dev_id),
             }
         } else {
             Err(())
-        }
+        };
     }
 
-    pub fn handle_power_requests(&mut self) -> Result<u16, ()>{
+    pub fn handle_power_requests(&mut self) -> Result<u16, ()> {
         let mut i = 0;
         while let Ok((switch_id, switch_state)) = self.switch_rx.recv() {
             match switch_state {
-                SwitchState::Off => {
-                    match self.send_power_off(switch_id) {
-                        Ok(_) => {
-                            i = i + 1;
-                        }
-                        Err(_) => {
-                            return Err(());
-                        }
+                SwitchState::Off => match self.send_power_off(switch_id) {
+                    Ok(_) => {
+                        i = i + 1;
                     }
-                }
-                SwitchState::On => {
-                    match self.send_power_on(switch_id) {
-                        Ok(_) => {
-                            i = i + 1;
-                        }
-                        Err(_) => {
-                            return Err(());
-                        }
+                    Err(_) => {
+                        return Err(());
                     }
-                }
+                },
+                SwitchState::On => match self.send_power_on(switch_id) {
+                    Ok(_) => {
+                        i = i + 1;
+                    }
+                    Err(_) => {
+                        return Err(());
+                    }
+                },
                 _ => {}
             }
-            self.update_switch_states(switch_id);
+            self.update_switch_states(switch_id).unwrap();
         }
         return Ok(i);
     }
 }
 
-pub fn core_power_task(switch_rx: Receiver<(SwitchId, SwitchState)>, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>, device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>) {
+pub fn core_power_task(
+    switch_rx: Receiver<(SwitchId, SwitchState)>,
+    can_tx: Sender<PackageModel>,
+    can_rx: Receiver<PackageModel>,
+    device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>,
+) {
     let mut pcdu = PCDU::new(switch_rx, can_tx, can_rx, device_state_map);
-    loop{
+    loop {
         pcdu.handle_power_requests().unwrap();
     }
-}
+}

src/pus.rs

@@ -14,8 +14,8 @@ use satrs_core::res_code::ResultU16;
 use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
 use satrs_core::tmtc::{AddressableId, PusServiceProvider};
 use satrs_core::{
-    spacepackets::ecss::PusPacket, spacepackets::tc::PusTc,
-    spacepackets::time::cds::TimeProvider, spacepackets::time::TimeWriter, spacepackets::SpHeader,
+    spacepackets::ecss::PusPacket, spacepackets::tc::PusTc, spacepackets::time::cds::TimeProvider,
+    spacepackets::time::TimeWriter, spacepackets::SpHeader,
 };
 use std::cell::RefCell;
 
@@ -26,6 +26,7 @@ use satrs_core::pus::scheduling::PusScheduler;
 use std::collections::HashMap;
 use std::rc::Rc;
 use std::sync::mpsc::Sender;
+use log::{debug};
 
 pub struct PusReceiver {
     pub tm_helper: PusTmWithCdsShortHelper,
@@ -108,8 +109,8 @@ impl PusServiceProvider for PusReceiver {
 impl PusReceiver {
     fn handle_test_service(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
         if PusPacket::subservice(pus_tc) == 1 {
-            println!("Received PUS ping command TC[17,1]");
-            println!("Sending ping reply PUS TM[17,2]");
+            debug!("Received PUS ping command TC[17,1]");
+            debug!("Sending ping reply PUS TM[17,2]");
             let ping_reply = self.tm_helper.create_pus_tm_timestamp_now(17, 2, None);
             let addr = self.tm_store.add_pus_tm(&ping_reply);
             let start_token = self

src/tmtc.rs

@@ -16,7 +16,7 @@ use crate::pus::PusReceiver;
 use crate::requests::RequestWithToken;
 use satrs_core::pool::{SharedPool, StoreAddr, StoreError};
 use satrs_core::pus::event_man::EventRequestWithToken;
-use satrs_core::pus::scheduling::PusScheduler;
+use satrs_core::pus::scheduling::{PusScheduler, TcInfo};
 use satrs_core::pus::verification::StdVerifReporterWithSender;
 use satrs_core::spacepackets::{ecss::PusPacket, tc::PusTc, tm::PusTm, SpHeader};
 use satrs_core::tmtc::{
@@ -161,11 +161,11 @@ impl ReceivesCcsdsTc for PusTcSource {
     }
 }
 pub fn core_tmtc_task(args: OtherArgs, mut tc_args: TcArgs, tm_args: TmArgs) {
-    let mut scheduler = Rc::new(RefCell::new(
+    let scheduler = Rc::new(RefCell::new(
         PusScheduler::new_with_current_init_time(Duration::from_secs(5)).unwrap(),
     ));
 
-    let mut sched_clone = scheduler.clone();
+    let sched_clone = scheduler.clone();
     let mut pus_receiver = PusReceiver::new(
         PUS_APID,
         tm_args.tm_sink_sender,
@@ -196,7 +196,7 @@ pub fn core_tmtc_task(args: OtherArgs, mut tc_args: TcArgs, tm_args: TmArgs) {
 
     let mut tc_buf: [u8; 4096] = [0; 4096];
     loop {
-        let mut tmtc_sched = scheduler.clone();
+        let tmtc_sched = scheduler.clone();
         core_tmtc_loop(
             &mut udp_tmtc_server,
             &mut tc_args,
@@ -219,11 +219,15 @@ fn core_tmtc_loop(
     pus_receiver: &mut PusReceiver,
     scheduler: Rc<RefCell<PusScheduler>>,
 ) {
-    let releaser = |enabled: bool, addr: &StoreAddr| -> bool {
-        match tc_args.tc_source.tc_source.send(*addr) {
-            Ok(_) => true,
-            Err(_) => false,
+    let releaser = |enabled: bool, info: &TcInfo| -> bool {
+        if enabled {
+            tc_args
+                .tc_source
+                .tc_source
+                .send(info.addr())
+                .expect("sending TC to TC source failed");
         }
+        true
     };
 
     let mut pool = tc_args
@@ -235,15 +239,11 @@ fn core_tmtc_loop(
 
     let mut scheduler = scheduler.borrow_mut();
     scheduler.update_time_from_now().unwrap();
-    match scheduler.release_telecommands(releaser, pool.as_mut()) {
-        Ok(released_tcs) => {
-            if released_tcs > 0 {
-                println!("{} Tc(s) released from scheduler", released_tcs);
-            }
+    if let Ok(released_tcs) = scheduler.release_telecommands(releaser, pool.as_mut()) {
+        if released_tcs > 0 {
+            println!("{released_tcs} TC(s) released from scheduler");
         }
-        Err(_) => {}
     }
-    //.expect("error releasing tc");
     drop(pool);
     drop(scheduler);