Cargo.lock

@@ -597,6 +597,26 @@ dependencies = [
  "libc",
 ]
 
+[[package]]
+name = "num_enum"
+version = "0.5.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8d829733185c1ca374f17e52b762f24f535ec625d2cc1f070e34c8a9068f341b"
+dependencies = [
+ "num_enum_derive",
+]
+
+[[package]]
+name = "num_enum_derive"
+version = "0.5.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2be1598bf1c313dcdd12092e3f1920f463462525a21b7b4e11b4168353d0123e"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
 [[package]]
 name = "once_cell"
 version = "1.16.0"
@@ -792,14 +812,15 @@ dependencies = [
 
 [[package]]
 name = "spacepackets"
-version = "0.5.2"
+version = "0.5.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a31966b4c2a1ca1902c6d34050648e4fbffeb124144775396422a94340068efd"
+checksum = "81541fd89a5bc02845a849895d6ed1721235b3eac26fc77010f0a05f53bc4e8a"
 dependencies = [
  "chrono",
  "crc",
  "delegate",
  "num-traits",
+ "num_enum",
  "serde",
  "zerocopy",
 ]

pyclient/main copy.py

@@ -0,0 +1,383 @@
+#!/usr/bin/env python3
+"""Example client for the sat-rs example application"""
+import enum
+import logging
+import struct
+import sys
+import time
+from typing import Optional
+import datetime
+
+import tmtccmd
+from spacepackets.ecss import PusTelemetry, PusTelecommand, PusVerificator
+from spacepackets.ecss.pus_17_test import Service17Tm
+from spacepackets.ecss.pus_1_verification import UnpackParams, Service1Tm
+from spacepackets.ccsds.time import CdsShortTimestamp
+
+from tmtccmd import CcsdsTmtcBackend, TcHandlerBase, ProcedureParamsWrapper
+from tmtccmd.tc.pus_3_fsfw_hk import generate_one_hk_command, make_sid
+from tmtccmd.tc.pus_11_tc_sched import create_time_tagged_cmd
+from tmtccmd.core.base import BackendRequest
+from tmtccmd.pus import VerificationWrapper
+from tmtccmd.tm import CcsdsTmHandler, SpecificApidHandlerBase
+from tmtccmd.com import ComInterface
+from tmtccmd.config import (
+    default_json_path,
+    SetupParams,
+    HookBase,
+    TmtcDefinitionWrapper,
+    CoreServiceList,
+    OpCodeEntry,
+    params_to_procedure_conversion,
+)
+from tmtccmd.config import PreArgsParsingWrapper, SetupWrapper
+from tmtccmd.logging import add_colorlog_console_logger
+from tmtccmd.logging.pus import (
+    RegularTmtcLogWrapper,
+    RawTmtcTimedLogWrapper,
+    TimedLogWhen,
+)
+from tmtccmd.tc import (
+    TcQueueEntryType,
+    ProcedureWrapper,
+    TcProcedureType,
+    FeedWrapper,
+    SendCbParams,
+    DefaultPusQueueHelper,
+    QueueWrapper,
+)
+from tmtccmd.tm.pus_5_fsfw_event import Service5Tm
+from tmtccmd.util import FileSeqCountProvider, PusFileSeqCountProvider
+from tmtccmd.util.obj_id import ObjectIdDictT
+
+from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
+
+_LOGGER = logging.getLogger(__name__)
+
+EXAMPLE_PUS_APID = 0x02
+
+
+class SatRsConfigHook(HookBase):
+    def __init__(self, json_cfg_path: str):
+        super().__init__(json_cfg_path=json_cfg_path)
+
+    def assign_communication_interface(self, com_if_key: str) -> Optional[ComInterface]:
+        from tmtccmd.config.com import (
+            create_com_interface_default,
+            create_com_interface_cfg_default,
+        )
+
+        cfg = create_com_interface_cfg_default(
+            com_if_key=com_if_key,
+            json_cfg_path=self.cfg_path,
+            space_packet_ids=None,
+        )
+        return create_com_interface_default(cfg)
+
+    def get_tmtc_definitions(self) -> TmtcDefinitionWrapper:
+        from tmtccmd.config.globals import get_default_tmtc_defs
+
+        defs = get_default_tmtc_defs()
+        srv_5 = OpCodeEntry()
+        srv_5.add("0", "Event Test")
+        defs.add_service(
+            name=CoreServiceList.SERVICE_5.value,
+            info="PUS Service 5 Event",
+            op_code_entry=srv_5,
+        )
+        srv_17 = OpCodeEntry()
+        srv_17.add("0", "Ping Test")
+        defs.add_service(
+            name=CoreServiceList.SERVICE_17_ALT,
+            info="PUS Service 17 Test",
+            op_code_entry=srv_17,
+        )
+        srv_3 = OpCodeEntry()
+        srv_3.add(HkOpCodes.GENERATE_ONE_SHOT, "Generate AOCS one shot HK")
+        defs.add_service(
+            name=CoreServiceList.SERVICE_3,
+            info="PUS Service 3 Housekeeping",
+            op_code_entry=srv_3,
+        )
+
+        srv_8 = OpCodeEntry()
+        srv_8.add("1", "Camera Image Request")
+        defs.add_service(
+            name=CoreServiceList.SERVICE_8,
+            info="PUS Service 8 Action",
+            op_code_entry=srv_8,
+        )
+
+        srv_11 = OpCodeEntry()
+        srv_11.add("0", "Scheduled TC Test")
+        defs.add_service(
+            name=CoreServiceList.SERVICE_11,
+            info="PUS Service 11 TC Scheduling",
+            op_code_entry=srv_11,
+        )
+        return defs
+
+    def perform_mode_operation(self, tmtc_backend: CcsdsTmtcBackend, mode: int):
+        _LOGGER.info("Mode operation hook was called")
+        pass
+
+    def get_object_ids(self) -> ObjectIdDictT:
+        from tmtccmd.config.objects import get_core_object_ids
+
+        return get_core_object_ids()
+
+
+class PusHandler(SpecificApidHandlerBase):
+    def __init__(
+        self,
+        verif_wrapper: VerificationWrapper,
+        printer: FsfwTmTcPrinter,
+        raw_logger: RawTmtcTimedLogWrapper,
+    ):
+        super().__init__(EXAMPLE_PUS_APID, None)
+        self.printer = printer
+        self.raw_logger = raw_logger
+        self.verif_wrapper = verif_wrapper
+
+    def handle_tm(self, packet: bytes, _user_args: any):
+        try:
+            tm_packet = PusTelemetry.unpack(
+                packet, time_reader=CdsShortTimestamp.empty()
+            )
+        except ValueError as e:
+            _LOGGER.warning("Could not generate PUS TM object from raw data")
+            _LOGGER.warning(f"Raw Packet: [{packet.hex(sep=',')}], REPR: {packet!r}")
+            raise e
+        service = tm_packet.service
+        dedicated_handler = False
+        if service == 1:
+            tm_packet = Service1Tm.unpack(
+                data=packet, params=UnpackParams(CdsShortTimestamp.empty(), 1, 2)
+            )
+            res = self.verif_wrapper.add_tm(tm_packet)
+            if res is None:
+                _LOGGER.info(
+                    f"Received Verification TM[{tm_packet.service}, {tm_packet.subservice}] "
+                    f"with Request ID {tm_packet.tc_req_id.as_u32():#08x}"
+                )
+                _LOGGER.warning(
+                    f"No matching telecommand found for {tm_packet.tc_req_id}"
+                )
+            else:
+                self.verif_wrapper.log_to_console(tm_packet, res)
+                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=',')}]")
+            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:]
+            dedicated_handler = True
+        if service == 5:
+            tm_packet = Service5Tm.unpack(packet, time_reader=CdsShortTimestamp.empty())
+        if service == 17:
+            tm_packet = Service17Tm.unpack(
+                packet, time_reader=CdsShortTimestamp.empty()
+            )
+            dedicated_handler = True
+            if tm_packet.subservice == 2:
+                self.printer.file_logger.info("Received Ping Reply TM[17,2]")
+                _LOGGER.info("Received Ping Reply TM[17,2]")
+            else:
+                self.printer.file_logger.info(
+                    f"Received Test Packet with unknown subservice {tm_packet.subservice}"
+                )
+                _LOGGER.info(
+                    f"Received Test Packet with unknown subservice {tm_packet.subservice}"
+                )
+        if tm_packet is None:
+            _LOGGER.info(
+                f"The service {service} is not implemented in Telemetry Factory"
+            )
+            tm_packet = PusTelemetry.unpack(
+                packet, time_reader=CdsShortTimestamp.empty()
+            )
+        self.raw_logger.log_tm(tm_packet)
+        if not dedicated_handler and tm_packet is not None:
+            pass
+            # self.printer.handle_long_tm_print(packet_if=tm_packet, info_if=tm_packet)
+
+
+def make_addressable_id(target_id: int, unique_id: int) -> bytes:
+    byte_string = bytearray(struct.pack("!I", target_id))
+    byte_string.extend(struct.pack("!I", unique_id))
+    return byte_string
+
+
+def read_addressable_id(data: bytes) -> tuple[int, int]:
+    target_id = struct.unpack("!I", data[0:4])[0]
+    set_id = struct.unpack("!I", data[4:8])[0]
+    return (target_id, set_id)
+
+
+class RequestTargetId(enum.IntEnum):
+    ACS = 1
+    PLD = 2
+
+
+class AcsHkIds(enum.IntEnum):
+    MGM_SET = 1
+
+
+class HkOpCodes:
+    GENERATE_ONE_SHOT = ["0", "oneshot"]
+
+
+def make_target_id(target_id: int) -> bytes:
+    byte_string = bytearray(struct.pack("!I", target_id))
+    return byte_string
+
+
+class TcHandler(TcHandlerBase):
+    def __init__(
+        self,
+        seq_count_provider: FileSeqCountProvider,
+        verif_wrapper: VerificationWrapper,
+    ):
+        super(TcHandler, self).__init__()
+        self.seq_count_provider = seq_count_provider
+        self.verif_wrapper = verif_wrapper
+        self.queue_helper = DefaultPusQueueHelper(
+            queue_wrapper=QueueWrapper.empty(),
+            tc_sched_timestamp_len=CdsShortTimestamp.TIMESTAMP_SIZE,
+            seq_cnt_provider=seq_count_provider,
+            pus_verificator=self.verif_wrapper.pus_verificator,
+            default_pus_apid=EXAMPLE_PUS_APID,
+        )
+
+    def send_cb(self, send_params: SendCbParams):
+        entry_helper = send_params.entry
+        if entry_helper.is_tc:
+            if entry_helper.entry_type == TcQueueEntryType.PUS_TC:
+                pus_tc_wrapper = entry_helper.to_pus_tc_entry()
+                raw_tc = pus_tc_wrapper.pus_tc.pack()
+                _LOGGER.info(f"Sending {pus_tc_wrapper.pus_tc}")
+                send_params.com_if.send(raw_tc)
+        elif entry_helper.entry_type == TcQueueEntryType.LOG:
+            log_entry = entry_helper.to_log_entry()
+            _LOGGER.info(log_entry.log_str)
+
+    def queue_finished_cb(self, helper: ProcedureWrapper):
+        if helper.proc_type == TcProcedureType.DEFAULT:
+            def_proc = helper.to_def_procedure()
+            _LOGGER.info(
+                f"Queue handling finished for service {def_proc.service} and "
+                f"op code {def_proc.op_code}"
+            )
+
+    def feed_cb(self, helper: ProcedureWrapper, wrapper: FeedWrapper):
+        q = self.queue_helper
+        q.queue_wrapper = wrapper.queue_wrapper
+        if helper.proc_type == TcProcedureType.DEFAULT:
+            def_proc = helper.to_def_procedure()
+            service = def_proc.service
+            op_code = def_proc.op_code
+            if (
+                service == CoreServiceList.SERVICE_17
+                or service == CoreServiceList.SERVICE_17_ALT
+            ):
+                q.add_log_cmd("Sending PUS ping telecommand")
+                return q.add_pus_tc(PusTelecommand(service=17, subservice=1))
+            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 service == CoreServiceList.SERVICE_8:
+                q.add_log_cmd("Sending PUS action request telecommand")
+                return q.add_pus_tc(
+                    PusTelecommand(service=8, subservice=1, app_data=make_target_id(RequestTargetId.PLD))
+                )
+            if service == CoreServiceList.SERVICE_3:
+                if op_code in HkOpCodes.GENERATE_ONE_SHOT:
+                    q.add_log_cmd("Sending HK one shot request")
+                    q.add_pus_tc(
+                        generate_one_hk_command(
+                            make_addressable_id(RequestTargetId.ACS, AcsHkIds.MGM_SET)
+                        )
+                    )
+                pass
+
+
+def main():
+    add_colorlog_console_logger(_LOGGER)
+    tmtccmd.init_printout(False)
+    hook_obj = SatRsConfigHook(json_cfg_path=default_json_path())
+    parser_wrapper = PreArgsParsingWrapper()
+    parser_wrapper.create_default_parent_parser()
+    parser_wrapper.create_default_parser()
+    parser_wrapper.add_def_proc_args()
+    params = SetupParams()
+    post_args_wrapper = parser_wrapper.parse(hook_obj, params)
+    proc_wrapper = ProcedureParamsWrapper()
+    if post_args_wrapper.use_gui:
+        post_args_wrapper.set_params_without_prompts(proc_wrapper)
+    else:
+        post_args_wrapper.set_params_with_prompts(proc_wrapper)
+    params.apid = EXAMPLE_PUS_APID
+    setup_args = SetupWrapper(
+        hook_obj=hook_obj, setup_params=params, proc_param_wrapper=proc_wrapper
+    )
+    # Create console logger helper and file loggers
+    tmtc_logger = RegularTmtcLogWrapper()
+    printer = FsfwTmTcPrinter(tmtc_logger.logger)
+    raw_logger = RawTmtcTimedLogWrapper(when=TimedLogWhen.PER_HOUR, interval=1)
+    verificator = PusVerificator()
+    verification_wrapper = VerificationWrapper(
+        verificator, _LOGGER, printer.file_logger
+    )
+    # Create primary TM handler and add it to the CCSDS Packet Handler
+    tm_handler = PusHandler(verification_wrapper, printer, raw_logger)
+    ccsds_handler = CcsdsTmHandler(generic_handler=None)
+    ccsds_handler.add_apid_handler(tm_handler)
+
+    # Create TC handler
+    seq_count_provider = PusFileSeqCountProvider()
+    tc_handler = TcHandler(seq_count_provider, verification_wrapper)
+    tmtccmd.setup(setup_args=setup_args)
+    init_proc = params_to_procedure_conversion(setup_args.proc_param_wrapper)
+    tmtc_backend = tmtccmd.create_default_tmtc_backend(
+        setup_wrapper=setup_args,
+        tm_handler=ccsds_handler,
+        tc_handler=tc_handler,
+        init_procedure=init_proc,
+    )
+    tmtccmd.start(tmtc_backend=tmtc_backend, hook_obj=hook_obj)
+    try:
+        while True:
+            state = tmtc_backend.periodic_op(None)
+            if state.request == BackendRequest.TERMINATION_NO_ERROR:
+                sys.exit(0)
+            elif state.request == BackendRequest.DELAY_IDLE:
+                _LOGGER.info("TMTC Client in IDLE mode")
+                time.sleep(3.0)
+            elif state.request == BackendRequest.DELAY_LISTENER:
+                time.sleep(0.8)
+            elif state.request == BackendRequest.DELAY_CUSTOM:
+                if state.next_delay.total_seconds() <= 0.4:
+                    time.sleep(state.next_delay.total_seconds())
+                else:
+                    time.sleep(0.4)
+            elif state.request == BackendRequest.CALL_NEXT:
+                pass
+    except KeyboardInterrupt:
+        sys.exit(0)
+
+
+if __name__ == "__main__":
+    main()

pyclient/main.py

@@ -218,7 +218,7 @@ def make_target_id(target_id: int) -> bytes:
     byte_string = bytearray(struct.pack("!I", target_id))
     return byte_string
 
-def read_addressable_id(data: bytes) -> Tuple[int, int]:
+def read_addressable_id(data: bytes) -> tuple[int, int]:
     target_id = struct.unpack("!I", data[0:4])[0]
     set_id = struct.unpack("!I", data[4:8])[0]
     return (target_id, set_id)
@@ -248,6 +248,7 @@ class TcHandler(TcHandlerBase):
         self.verif_wrapper = verif_wrapper
         self.queue_helper = DefaultPusQueueHelper(
             queue_wrapper=None,
+            tc_sched_timestamp_len=7,
             seq_cnt_provider=seq_count_provider,
         )
 

pyclient/requirements.txt

@@ -1,2 +1,2 @@
-# tmtccmd == 3.0.0
--e git+https://github.com/robamu-org/tmtccmd@97e5e51101a08b21472b3ddecc2063359f7e307a#egg=tmtccmd
+tmtccmd == 4.0.0rc0
+# -e git+https://github.com/robamu-org/tmtccmd@97e5e51101a08b21472b3ddecc2063359f7e307a#egg=tmtccmd

src/action.rs

@@ -1,5 +1,4 @@
 use eurosim_obsw::RequestTargetId;
-use satrs_core::tmtc::AddressableId;
 
 pub type CollectionIntervalFactor = u32;
 

src/can.rs

@@ -2,7 +2,7 @@
 #![allow(unused_variables)]
 #![allow(unused_imports)]
 
-use crate::device_handler::DeviceState;
+use crate::pcdu::DeviceState;
 
 use crate::can_ids::{
     can_id_to_package_id, package_id_to_can_id, value_to_package_id, DeviceId, PackageId,

src/can_ids.rs

@@ -1,9 +1,6 @@
 use embedded_can::{Id, StandardId};
 use log::warn;
 use std::collections::HashMap;
-use std::fs;
-use std::sync::mpsc::Sender;
-use std::thread::Thread;
 
 pub use num_derive::{FromPrimitive, ToPrimitive};
 pub use num_traits::{FromPrimitive, ToPrimitive};
@@ -93,6 +90,39 @@ pub enum DeviceId {
     All = 23,
 }
 
+impl TryFrom<u16> for DeviceId {
+    type Error = ();
+
+    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),
+            _ => Err(()),
+        }
+    }
+}
+
 #[derive(Debug)]
 pub struct PackageModel {
     package_id: PackageId,

src/device_handler.rs

@@ -1,222 +0,0 @@
-use crate::can_ids::{DeviceId, PackageId, PackageModel, ThreadId};
-use log::{info, warn};
-use socketcan::{errors, frame, socket, CanFrame, Socket};
-use std::collections::HashMap;
-use std::hash::Hash;
-use std::sync::mpsc::{Receiver, RecvError, Sender};
-
-use crate::can::{CanRxHandler, CanTxHandler};
-pub use num_derive::{FromPrimitive, ToPrimitive};
-pub use num_traits::{FromPrimitive, ToPrimitive};
-pub use strum::IntoEnumIterator; // 0.17.1
-pub use strum_macros::EnumIter; // 0.17.1
-
-#[derive(Copy, Clone, Debug, PartialEq)]
-pub enum DeviceState {
-    On,
-    Off,
-    Broken,
-    Unknown,
-}
-
-pub struct CanDeviceHandler {
-    device_state_map: HashMap<DeviceId, DeviceState>,
-    can_tx_handler: CanTxHandler,
-    can_rx_receiver: Receiver<PackageModel>,
-}
-
-impl CanDeviceHandler {
-    pub fn new(
-        can_tx_handler: CanTxHandler,
-        can_rx_receiver: Receiver<PackageModel>,
-    ) -> CanDeviceHandler {
-        let mut device_state_map: HashMap<DeviceId, DeviceState> = HashMap::new();
-
-        for id in DeviceId::iter() {
-            device_state_map.insert(id, DeviceState::Unknown);
-        }
-        CanDeviceHandler {
-            device_state_map,
-            can_tx_handler,
-            can_rx_receiver,
-        }
-    }
-
-    pub fn power_on(&mut self, id: DeviceId) -> Result<(), ()> {
-        if !self.device_state_map.contains_key(&id) {
-            return Err(());
-        }
-        info!("Powering on device {:?}", id);
-
-        let msg_data: [u8; 1] = [id as u8];
-        self.can_tx_handler
-            .tx_socket(PackageId::DevicePowerOnRequest, &msg_data);
-
-        let request_confirmation = self.can_rx_receiver.recv();
-        match request_confirmation {
-            Ok(confirmation) => {
-                if confirmation.package_id() != PackageId::DevicePowerOnRequestConfirmation {
-                    warn!("Wrong package ID.");
-                    return Err(());
-                }
-                if confirmation.data()[0] != id as u8 {
-                    warn!("Wrong device ID.");
-                    return Err(());
-                }
-                if confirmation.data()[1] != 1 {
-                    warn!("Request unsuccessful.");
-                    return Err(());
-                }
-                info!("Power on request confirmation received.");
-            }
-            Err(_) => {
-                warn!("Error receiving package.");
-                return Err(());
-            }
-        }
-
-        let request_success = self.can_rx_receiver.recv();
-        match request_success {
-            Ok(success) => {
-                if success.package_id() != PackageId::DevicePowerOnConfirmation {
-                    warn!("Wrong package ID.");
-                    return Err(());
-                }
-                if success.data()[0] != id as u8 {
-                    warn!("Wrong device ID.");
-                    return Err(());
-                }
-                if success.data()[1] != 1 {
-                    warn!("Power on unsuccessful.");
-                    return Err(());
-                }
-                info!("Power on confirmation received.");
-            }
-            Err(_) => {
-                warn!("Error receiving package.");
-                return Err(());
-            }
-        }
-
-        Ok(())
-    }
-
-    pub fn power_off(&mut self, id: DeviceId) -> Result<(), ()> {
-        if !self.device_state_map.contains_key(&id) {
-            return Err(());
-        }
-        info!("Powering on device {:?}", id);
-
-        let msg_data: [u8; 1] = [id as u8];
-        self.can_tx_handler
-            .tx_socket(PackageId::DevicePowerOffRequest, &msg_data);
-
-        let request_confirmation = self.can_rx_receiver.recv();
-        match request_confirmation {
-            Ok(confirmation) => {
-                if confirmation.package_id() != PackageId::DevicePowerOffRequestConfirmation {
-                    warn!("Wrong package ID.");
-                    return Err(());
-                }
-                if confirmation.data()[0] != id as u8 {
-                    warn!("Wrong device ID.");
-                    return Err(());
-                }
-                if confirmation.data()[1] != 1 {
-                    warn!("Request unsuccessful.");
-                    return Err(());
-                }
-                info!("Power off request confirmation received.");
-            }
-            Err(_) => {
-                warn!("Error receiving package.");
-                return Err(());
-            }
-        }
-
-        let request_success = self.can_rx_receiver.recv();
-        match request_success {
-            Ok(success) => {
-                if success.package_id() != PackageId::DevicePowerOffConfirmation {
-                    warn!("Wrong package ID.");
-                    return Err(());
-                }
-                if success.data()[0] != id as u8 {
-                    warn!("Wrong device ID.");
-                    return Err(());
-                }
-                if success.data()[1] != 1 {
-                    warn!("Power off unsuccessful.");
-                    return Err(());
-                }
-                info!("Power off confirmation received.");
-            }
-            Err(_) => {
-                warn!("Error receiving package.");
-                return Err(());
-            }
-        }
-
-        Ok(())
-    }
-
-    pub fn get_power_states(&mut self) -> HashMap<DeviceId, DeviceState> {
-        for id in DeviceId::iter() {
-            self.update_power_state(id)
-                .expect("Error updating power state.");
-        }
-        self.device_state_map.clone()
-    }
-
-    pub fn get_power_state(&mut self, id: DeviceId) -> Option<&DeviceState> {
-        self.update_power_state(id)
-            .expect("Error updating power state.");
-        self.device_state_map.get(&id)
-    }
-
-    pub fn update_power_state(&mut self, id: DeviceId) -> Result<(), ()> {
-        if !self.device_state_map.contains_key(&id) {
-            return Err(());
-        }
-
-        let msg_data: [u8; 1] = [id as u8];
-        self.can_tx_handler
-            .tx_socket(PackageId::DevicePowerStatusRequest, &msg_data);
-
-        let response = self.can_rx_receiver.recv();
-        if let Ok(response) = response {
-            let data = response.data();
-
-            if data[0] == id as u8 {
-                if data[1] == 1 {
-                    *self.device_state_map.get_mut(&id).unwrap() = DeviceState::On;
-                } else if data[1] == 0 {
-                    *self.device_state_map.get_mut(&id).unwrap() = DeviceState::Off;
-                } else {
-                    return Err(());
-                }
-            }
-        } else {
-            return Err(());
-        }
-        Ok(())
-    }
-}
-
-pub fn power_up_sequence(mut device_handler: CanDeviceHandler) -> HashMap<DeviceId, DeviceState> {
-    for id in DeviceId::iter() {
-        device_handler
-            .power_on(id)
-            .expect("Error powering on device.");
-    }
-    device_handler.get_power_states()
-}
-
-pub fn power_down_sequence(mut device_handler: CanDeviceHandler) -> HashMap<DeviceId, DeviceState> {
-    for id in DeviceId::iter() {
-        device_handler
-            .power_off(id)
-            .expect("Error powering on device.");
-    }
-    device_handler.get_power_states()
-}

src/main.rs

@@ -4,14 +4,14 @@ mod cam;
 mod can;
 mod can_ids;
 mod ccsds;
-mod device_handler;
+mod pld_handler;
 mod hk;
 mod logger;
 mod pus;
 mod requests;
 mod tmtc;
+mod pcdu;
 
-use crate::hk::{AcsHkIds, HkRequest};
 use crate::requests::{Request, RequestWithToken};
 use crate::tmtc::{
     core_tmtc_task, OtherArgs, PusTcSource, TcArgs, TcStore, TmArgs, TmFunnel, TmStore, PUS_APID,
@@ -23,37 +23,37 @@ 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, EventRequest, EventRequestWithToken,
+    DefaultPusMgmtBackendProvider, EventReporter, EventRequestWithToken,
     PusEventDispatcher,
 };
-use satrs_core::pus::hk::Subservice;
 use satrs_core::pus::verification::{
     MpscVerifSender, VerificationReporterCfg, VerificationReporterWithSender,
 };
-use satrs_core::pus::{EcssTmError, EcssTmErrorWithSend, EcssTmSenderCore};
+use satrs_core::pus::{EcssTmErrorWithSend, EcssTmSenderCore};
 use satrs_core::seq_count::{
-    SeqCountProviderSyncClonable, SequenceCountProvider, SequenceCountProviderCore,
+    SeqCountProviderSyncClonable,
 };
 use satrs_core::{
     spacepackets::time::cds::TimeProvider,
     spacepackets::time::TimeWriter,
-    spacepackets::tm::{PusTm, PusTmSecondaryHeader},
-    spacepackets::{SequenceFlags, SpHeader},
+    spacepackets::tm::{PusTm},
 };
+use strum::IntoEnumIterator;
 
-use crate::can_ids::{can_id_to_package_id, load_package_ids, PackageId, PackageModel, ThreadId};
+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 satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
 use std::collections::HashMap;
 use std::net::{IpAddr, SocketAddr};
-use std::sync::mpsc::{channel, RecvError, TryRecvError};
-use std::sync::{mpsc, Arc, RwLock};
+use std::sync::mpsc::{channel};
+use std::sync::{mpsc, Arc, RwLock, Mutex};
 use std::thread;
-use std::time::Duration;
+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};
 
 #[derive(Clone)]
 struct EventTmSender {
@@ -189,17 +189,6 @@ fn main() {
     // get package id hashmap
     let package_ids_rx = load_package_ids();
 
-    // checks for packet ids
-    println!("{:?}", package_ids_rx[&PackageId::PCDUStatusRequest]);
-    println!(
-        "{:?}",
-        package_ids_rx[&PackageId::CameraImageRequestConfirmation]
-    );
-    let test = can_id_to_package_id(Id::Standard(StandardId::new(65).expect("Invalid Id")));
-    if let Some(id) = test {
-        println!("{:?}", package_ids_rx[&id]);
-    }
-
     let socket0 = can::CanRxHandler::new_socket("can0", can_senders, package_ids_rx).unwrap();
 
     info!("Starting TMTC task");
@@ -217,6 +206,26 @@ fn main() {
         }
     });
 
+    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 mut device_state_map = HashMap::new();
+        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());
+
+    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());
+    });
+
+
     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();
@@ -319,106 +328,12 @@ fn main() {
 
     let PLDCanSocket =
         can::CanTxHandler::new_socket("can0", ThreadId::PLDThread, package_map_pld_tx).unwrap();
+
+    //let mut pcdu_tx_clone = pcdu_tx.clone();
     println!("Starting Payload Handling task");
     let builder3 = thread::Builder::new().name("PLDThread".into());
     let jh3 = builder3.spawn(move || {
-        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);
-                                    // get current time stamp
-                                    let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
-                                    cds_stamp.write_to_bytes(&mut time_stamp_buf);
-
-                                    // 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.");
-
-                                    // make can bus package to camera
-                                    let data = [1];
-                                    PLDCanSocket.tx_socket(PackageId::CameraImageRequest, &data);
-
-                                    //let timeout = Duration::from_millis(400);
-
-                                    // loop to allow early exit incase verif never arrives
-
-                                    // wait for request verification
-                                    loop {
-                                        match pld_can_rx.recv() {
-                                            Ok(msg) => {
-                                                if msg.package_id()
-                                                    == PackageId::CameraImageRequestConfirmation
-                                                    && msg.data()[0] == 1
-                                                {
-                                                    break;
-                                                }
-                                            }
-                                            Err(_) => {
-                                                warn!("Error receiving Can Bus Message.");
-                                            }
-                                        }
-                                    }
-                                    // wait for start of execution
-                                    loop {
-                                        match pld_can_rx.recv() {
-                                            Ok(msg) => {
-                                                if msg.package_id()
-                                                    == PackageId::CameraImageExecutionStart
-                                                    && msg.data()[0] == 1
-                                                {
-                                                    break;
-                                                }
-                                            }
-                                            Err(_) => {
-                                                warn!("Error receiving Can Bus Message.");
-                                            }
-                                        }
-                                    }
-                                    // wait for end of execution
-                                    loop {
-                                        match pld_can_rx.recv() {
-                                            Ok(msg) => {
-                                                if msg.package_id()
-                                                    == PackageId::CameraImageExectutionEnd
-                                                    && msg.data()[0] == 1
-                                                {
-                                                    let cds_stamp =
-                                                        TimeProvider::from_now_with_u16_days()
-                                                            .unwrap();
-                                                    cds_stamp.write_to_bytes(&mut time_stamp_buf);
-
-                                                    // send end verification with token
-                                                    reporter_pld
-                                                        .completion_success(
-                                                            start_token,
-                                                            Some(&time_stamp_buf),
-                                                        )
-                                                        .expect("Error sending start success.");
-                                                    break;
-                                                }
-                                            }
-                                            Err(_) => {
-                                                warn!("Error receiving Can Bus Message.");
-                                            }
-                                        }
-                                    }
-                                }
-                                ActionRequest::OrientationRequest(_) => {}
-                                _ => {}
-                            }
-                        }
-                        _ => {}
-                    }
-                }
-                Err(_) => {}
-            }
-        }
+        core_pld_task(power_switcher.clone(), pld_thread_rx, pld_can_rx, &mut reporter_pld);
     });
 
     println!("Starting TM funnel task");
@@ -444,15 +359,9 @@ fn main() {
     jh1.unwrap()
         .join()
         .expect("Joining CAN Bus Listening thread failed");
-    //jh2.unwrap().join().expect("Joing AOCS thread failed");
-    jh3.unwrap().join().expect("Joing AOCS thread failed");
-    jh4.unwrap().join().expect("Joing AOCS thread failed");
-
-    /*
-    jh1.join().expect("Joining TM Funnel thread failed");
-    jh2.join().expect("Joining Event Manager thread failed");
-    jh3.join().expect("Joining AOCS 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");
 }
 #[derive(Default)]
 struct MgmData {

src/pcdu.rs

@@ -0,0 +1,225 @@
+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 std::convert::TryFrom;
+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)]
+pub enum DeviceState {
+    On,
+    Off,
+    SwitchingPower,
+    Setup,
+    Idle,
+}
+
+#[derive(Clone)]
+pub struct PowerSwitcher {
+    switch_tx: Sender<(SwitchId, SwitchState)>,
+    device_state_map: Arc<Mutex<HashMap<DeviceId, SwitchState>>>,
+}
+
+pub struct PCDU {
+    switch_rx: Receiver<(SwitchId, SwitchState)>,
+    can_tx: CanTxHandler,
+    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}
+    }
+}
+
+impl PowerSwitcherCommandSender for PowerSwitcher {
+    type Error = ();
+
+    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(())}
+        }
+    }
+
+    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(())}
+        }
+    }
+}
+
+impl PowerSwitchInfo for PowerSwitcher {
+    type Error = ();
+
+    fn get_switch_state(&mut self, switch_id: SwitchId) -> Result<SwitchState, Self::Error> {
+        let map_locked = self.device_state_map.lock().unwrap();
+        if let Ok(dev_id) = DeviceId::try_from(switch_id) {
+            return if let Some(state) = map_locked.get(&dev_id) {
+                Ok(*state)
+            } else {
+                Err(())
+            }
+        } else {
+            Err(())
+        }
+
+    }
+
+    fn switch_delay_ms(&self) -> u32 {
+        0
+    }
+}
+
+impl PowerSwitchProvider for PowerSwitcher {
+    type Error = ();
+}
+
+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 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);
+            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();
+            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);
+            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();
+            Ok(())
+        } else {
+            Err(())
+        }
+    }
+
+    pub fn update_states_helper(&mut self, dev_id: &DeviceId) -> Result<(), ()> {
+        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);
+        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");
+                    let mut map_lock = self.device_state_map.lock().unwrap();
+                    let mut state: SwitchState;
+                    match msg.data()[1] {
+                        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);
+                    Ok(())
+                } else {
+                    Err(())
+                }
+            }
+            Err(_) => {
+                Err(())
+            }
+        }
+
+    }
+
+    pub fn update_all_states_helper(&mut self) -> Result<(), ()> {
+        let mut 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());
+        }
+        drop(map_lock);
+        for dev_id in device_list {
+            self.update_states_helper(&dev_id)?;
+        }
+        Ok(())
+    }
+
+    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)
+                }
+            }
+        } else {
+            Err(())
+        }
+    }
+
+    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::On => {
+                    match self.send_power_on(switch_id) {
+                        Ok(_) => {
+                            i = i + 1;
+                        }
+                        Err(_) => {
+                            return Err(());
+                        }
+                    }
+                }
+                _ => {}
+            }
+            self.update_switch_states(switch_id);
+        }
+        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>>>) {
+    let mut pcdu = PCDU::new(switch_rx, can_tx, can_rx, device_state_map);
+    loop{
+        pcdu.handle_power_requests().unwrap();
+    }
+}

src/pld_handler.rs

@@ -0,0 +1,171 @@
+use std::sync::mpsc;
+use std::sync::mpsc::{Receiver, Sender, TryRecvError};
+use log::info;
+use satrs_core::power::{PowerSwitcherCommandSender, PowerSwitchInfo, SwitchId, SwitchState};
+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};
+
+#[derive(Debug, PartialEq, Copy, Clone)]
+pub enum CameraMode {
+    Idle,
+    PictureRequest,
+    Verification,
+    Start,
+    End,
+}
+
+pub struct CameraHandler {
+    power_switcher: PowerSwitcher,
+    camera_device_id: DeviceId,
+    camera_switch_id: SwitchId,
+    device_state: DeviceState,
+    can_tx: CanTxHandler,
+    can_rx: Receiver<PackageModel>,
+    mode: CameraMode,
+    mode_rx: Receiver<CameraMode>,
+}
+
+impl CameraHandler {
+    pub fn new(power_switcher: PowerSwitcher, camera_device_id: DeviceId, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>, mode_rx: Receiver<CameraMode>) -> 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}
+    }
+
+    pub fn set_mode(&mut self, mode: CameraMode) {
+        if self.mode == CameraMode::Idle{
+            if mode == CameraMode::PictureRequest {
+                self.mode = PictureRequest;
+            }
+        }
+    }
+
+    pub fn get_mode(&self) -> CameraMode {
+        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 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() {
+            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.");
+
+                                info!("{:?}", camera_handler.get_mode());
+                                while camera_handler.get_mode() != CameraMode::Idle {
+                                    camera_handler.periodic_op();
+                                }
+
+                                // 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/pus.rs

@@ -1,10 +1,10 @@
 use crate::hk::{CollectionIntervalFactor, HkRequest};
 use crate::requests::{Request, RequestWithToken};
 use crate::tmtc::{PusTcSource, TmStore};
-use eurosim_obsw::{hk_err, tmtc_err, RequestTargetId};
+use eurosim_obsw::{hk_err, tmtc_err};
 use satrs_core::events::EventU32;
 use satrs_core::pool::StoreAddr;
-use satrs_core::pus::event::Subservices;
+use satrs_core::pus::event::Subservice;
 use satrs_core::pus::event_man::{EventRequest, EventRequestWithToken};
 use satrs_core::pus::hk;
 use satrs_core::pus::verification::{
@@ -14,7 +14,7 @@ use satrs_core::res_code::ResultU16;
 use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
 use satrs_core::tmtc::{AddressableId, PusServiceProvider};
 use satrs_core::{
-    spacepackets, spacepackets::ecss::PusPacket, spacepackets::tc::PusTc,
+    spacepackets::ecss::PusPacket, spacepackets::tc::PusTc,
     spacepackets::time::cds::TimeProvider, spacepackets::time::TimeWriter, spacepackets::SpHeader,
 };
 use std::cell::RefCell;
@@ -23,7 +23,6 @@ use crate::action;
 use crate::action::ActionRequest;
 use eurosim_obsw::RequestTargetId::{AcsSubsystem, PldSubsystem};
 use satrs_core::pus::scheduling::PusScheduler;
-use satrs_core::spacepackets::ecss::PusServiceId::Action;
 use std::collections::HashMap;
 use std::rc::Rc;
 use std::sync::mpsc::Sender;
@@ -188,13 +187,14 @@ impl PusReceiver {
                 .send(RequestWithToken(Request::HkRequest(request), token))
                 .unwrap_or_else(|_| panic!("Sending HK request {:?} failed", request));
         };
-        if PusPacket::subservice(pus_tc) == hk::Subservice::TcEnableGeneration as u8 {
+        if PusPacket::subservice(pus_tc) == hk::Subservice::TcEnableHkGeneration as u8 {
             send_request(HkRequest::Enable(addressable_id));
-        } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcDisableGeneration as u8 {
+        } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcDisableHkGeneration as u8 {
             send_request(HkRequest::Disable(addressable_id));
         } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcGenerateOneShotHk as u8 {
             send_request(HkRequest::OneShot(addressable_id));
-        } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcModifyCollectionInterval as u8
+        } else if PusPacket::subservice(pus_tc)
+            == hk::Subservice::TcModifyHkCollectionInterval as u8
         {
             if user_data.len() < 12 {
                 self.update_time_stamp();
@@ -257,7 +257,7 @@ impl PusReceiver {
         }
         let event_id = EventU32::from(u32::from_be_bytes(app_data.try_into().unwrap()));
         match PusPacket::subservice(pus_tc).try_into() {
-            Ok(Subservices::TcEnableEventGeneration) => {
+            Ok(Subservice::TcEnableEventGeneration) => {
                 self.update_time_stamp();
                 let start_token = send_start_acceptance(&mut self.verif_reporter, &self.time_stamp);
                 self.event_request_tx
@@ -267,7 +267,7 @@ impl PusReceiver {
                     })
                     .expect("Sending event request failed");
             }
-            Ok(Subservices::TcDisableEventGeneration) => {
+            Ok(Subservice::TcDisableEventGeneration) => {
                 self.update_time_stamp();
                 let start_token = send_start_acceptance(&mut self.verif_reporter, &self.time_stamp);
                 self.event_request_tx
@@ -417,8 +417,6 @@ impl PusReceiver {
                 scheduler
                     .insert_wrapped_tc::<TimeProvider>(pus_tc, pool.as_mut())
                     .expect("TODO: panic message");
-                let time =
-                    TimeProvider::from_bytes_with_u16_days(&pus_tc.user_data().unwrap()).unwrap();
                 drop(scheduler);
 
                 self.verif_reporter

src/requests.rs

@@ -1,5 +1,4 @@
 use crate::action::ActionRequest;
-use crate::cam::CameraRequest;
 use crate::hk::HkRequest;
 use satrs_core::pus::verification::{TcStateAccepted, VerificationToken};
 

src/tmtc.rs

@@ -8,7 +8,6 @@ use std::fmt::{Display, Formatter};
 use std::net::SocketAddr;
 use std::rc::Rc;
 use std::sync::mpsc::{Receiver, SendError, Sender, TryRecvError};
-use std::sync::{Arc, LockResult, Mutex};
 use std::thread;
 use std::time::Duration;