From 7ae4701e5f369205616b261aecbd09ddd2c5e281 Mon Sep 17 00:00:00 2001
From: lkoester <linus@koester-im-web.de>
Date: Wed, 18 Jan 2023 10:04:36 +0100
Subject: [PATCH] added stuff, preparing for change of packageids

---
 .../runConfigurations/Cross_Remote_Debug.xml  |  10 +
 .idea/runConfigurations/Run.xml               |   2 +-
 Cargo.lock                                    | 265 +++++++++-----
 Cargo.toml                                    |  10 +-
 pyclient/.gitignore                           |   6 +
 pyclient/main.py                              | 322 +++++++++++++++++
 pyclient/requirements.txt                     |   1 +
 pyclient/tmtc_conf.json                       |   6 +
 src/can.rs                                    | 241 ++++++++++--
 src/can_ids.rs                                | 258 +++++++++++++
 src/ccsds.rs                                  |  14 +-
 src/device_handler.rs                         |  37 ++
 src/example_main.rs                           | 243 +++++++++++++
 src/hk.rs                                     |  16 +
 src/lib.rs                                    |  48 ++-
 src/logger.rs                                 |  22 ++
 src/main.rs                                   | 342 ++++++++++++------
 src/pus.rs                                    | 151 ++++++--
 src/requests.rs                               |  10 +
 src/tmtc.rs                                   | 210 +++++++++--
 20 files changed, 1907 insertions(+), 307 deletions(-)
 create mode 100644 .idea/runConfigurations/Cross_Remote_Debug.xml
 create mode 100644 pyclient/.gitignore
 create mode 100644 pyclient/main.py
 create mode 100644 pyclient/requirements.txt
 create mode 100644 pyclient/tmtc_conf.json
 create mode 100644 src/can_ids.rs
 create mode 100644 src/device_handler.rs
 create mode 100644 src/example_main.rs
 create mode 100644 src/hk.rs
 create mode 100644 src/logger.rs
 create mode 100644 src/requests.rs

diff --git a/.idea/runConfigurations/Cross_Remote_Debug.xml b/.idea/runConfigurations/Cross_Remote_Debug.xml
new file mode 100644
index 0000000..f04441e
--- /dev/null
+++ b/.idea/runConfigurations/Cross_Remote_Debug.xml
@@ -0,0 +1,10 @@
+<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">
+      <debugger kind="GDB" isBundled="true" />
+    </custom-gdb-server>
+    <method v="2">
+      <option name="CLION.COMPOUND.BUILD" enabled="true" />
+    </method>
+  </configuration>
+</component>
\ No newline at end of file
diff --git a/.idea/runConfigurations/Run.xml b/.idea/runConfigurations/Run.xml
index fb73350..91993a3 100644
--- a/.idea/runConfigurations/Run.xml
+++ b/.idea/runConfigurations/Run.xml
@@ -3,7 +3,7 @@
     <option name="command" value="run --package eurosim-obsw --bin eurosim-obsw" />
     <option name="workingDirectory" value="file://$PROJECT_DIR$" />
     <option name="channel" value="DEFAULT" />
-    <option name="requiredFeatures" value="true" />
+    <option name="requiredFeatures" value="false" />
     <option name="allFeatures" value="false" />
     <option name="emulateTerminal" value="false" />
     <option name="withSudo" value="false" />
diff --git a/Cargo.lock b/Cargo.lock
index 4d85a20..77424fb 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -23,12 +23,12 @@ dependencies = [
 ]
 
 [[package]]
-name = "atomic-polyfill"
-version = "0.1.11"
+name = "array-init"
+version = "0.0.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e3ff7eb3f316534d83a8a2c3d1674ace8a5a71198eba31e2e2b597833f699b28"
+checksum = "23589ecb866b460d3a0f1278834750268c607e8e28a1b982c907219f3178cd72"
 dependencies = [
- "critical-section",
+ "nodrop",
 ]
 
 [[package]]
@@ -43,12 +43,30 @@ version = "0.13.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9e1b586273c5702936fe7b7d6896644d8be71e6314cfe09d3167c95f712589e8"
 
+[[package]]
+name = "bitflags"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8dead7461c1127cf637931a1e50934eb6eee8bff2f74433ac7909e9afcee04a3"
+
 [[package]]
 name = "bitflags"
 version = "1.3.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
 
+[[package]]
+name = "bstr"
+version = "0.2.17"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ba3569f383e8f1598449f1a423e72e99569137b47740b1da11ef19af3d5c3223"
+dependencies = [
+ "lazy_static",
+ "memchr",
+ "regex-automata",
+ "serde",
+]
+
 [[package]]
 name = "bumpalo"
 version = "3.11.1"
@@ -93,6 +111,7 @@ dependencies = [
  "iana-time-zone",
  "num-integer",
  "num-traits",
+ "serde",
  "winapi",
 ]
 
@@ -127,12 +146,6 @@ version = "2.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "2d0165d2900ae6778e36e80bbc4da3b5eefccee9ba939761f9c2882a5d9af3ff"
 
-[[package]]
-name = "critical-section"
-version = "1.1.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6548a0ad5d2549e111e1f6a11a6c2e2d00ce6a3dafe22948d67c2b443f775e52"
-
 [[package]]
 name = "crossbeam-channel"
 version = "0.5.6"
@@ -152,6 +165,28 @@ dependencies = [
  "cfg-if",
 ]
 
+[[package]]
+name = "csv"
+version = "1.1.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "22813a6dc45b335f9bade10bf7271dc477e81113e89eb251a0bc2a8a81c536e1"
+dependencies = [
+ "bstr",
+ "csv-core",
+ "itoa",
+ "ryu",
+ "serde",
+]
+
+[[package]]
+name = "csv-core"
+version = "0.1.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2b2466559f260f48ad25fe6317b3c8dac77b5bdb5763ac7d9d6103530663bc90"
+dependencies = [
+ "memchr",
+]
+
 [[package]]
 name = "cxx"
 version = "1.0.83"
@@ -196,17 +231,6 @@ dependencies = [
  "syn",
 ]
 
-[[package]]
-name = "delegate"
-version = "0.7.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d70a2d4995466955a415223acf3c9c934b9ff2339631cdf4ffc893da4bacd717"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn",
-]
-
 [[package]]
 name = "delegate"
 version = "0.8.0"
@@ -247,16 +271,8 @@ name = "eurosim-obsw"
 version = "0.1.0"
 dependencies = [
  "satrs-core",
- "spacepackets",
-]
-
-[[package]]
-name = "hash32"
-version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67"
-dependencies = [
- "byteorder",
+ "satrs-mib",
+ "socketcan",
 ]
 
 [[package]]
@@ -268,19 +284,6 @@ dependencies = [
  "ahash",
 ]
 
-[[package]]
-name = "heapless"
-version = "0.7.16"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "db04bc24a18b9ea980628ecf00e6c0264f3c1426dac36c00cb49b6fbad8b0743"
-dependencies = [
- "atomic-polyfill",
- "hash32",
- "rustc_version",
- "spin",
- "stable_deref_trait",
-]
-
 [[package]]
 name = "hermit-abi"
 version = "0.1.19"
@@ -290,6 +293,12 @@ dependencies = [
  "libc",
 ]
 
+[[package]]
+name = "hex"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d6a22814455d41612f41161581c2883c0c6a1c41852729b17d5ed88f01e153aa"
+
 [[package]]
 name = "iana-time-zone"
 version = "0.1.53"
@@ -314,6 +323,18 @@ dependencies = [
  "cxx-build",
 ]
 
+[[package]]
+name = "itertools"
+version = "0.4.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c4a9b56eb56058f43dc66e58f40a214b2ccbc9f3df51861b63d51dec7b65bc3f"
+
+[[package]]
+name = "itoa"
+version = "0.4.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b71991ff56294aa922b450139ee08b3bfc70982c6b2c7562771375cf73542dd4"
+
 [[package]]
 name = "js-sys"
 version = "0.3.60"
@@ -323,6 +344,12 @@ dependencies = [
  "wasm-bindgen",
 ]
 
+[[package]]
+name = "lazy_static"
+version = "1.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
+
 [[package]]
 name = "libc"
 version = "0.2.138"
@@ -338,16 +365,6 @@ dependencies = [
  "cc",
 ]
 
-[[package]]
-name = "lock_api"
-version = "0.4.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "435011366fe56583b16cf956f9df0095b405b82d76425bc8981c0e22e60ec4df"
-dependencies = [
- "autocfg",
- "scopeguard",
-]
-
 [[package]]
 name = "log"
 version = "0.4.17"
@@ -357,6 +374,34 @@ dependencies = [
  "cfg-if",
 ]
 
+[[package]]
+name = "maybe-uninit"
+version = "2.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "60302e4db3a61da70c0cb7991976248362f30319e88850c487b9b95bbf059e00"
+
+[[package]]
+name = "memchr"
+version = "2.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d"
+
+[[package]]
+name = "nix"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bfb3ddedaa14746434a02041940495bf11325c22f6d36125d3bdd56090d50a79"
+dependencies = [
+ "bitflags 0.4.0",
+ "libc",
+]
+
+[[package]]
+name = "nodrop"
+version = "0.1.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "72ef4a56884ca558e5ddb05a1d1e7e1bfd9a68d9ed024c21704cc98872dae1bb"
+
 [[package]]
 name = "num-integer"
 version = "0.1.45"
@@ -401,7 +446,7 @@ dependencies = [
  "cfg-if",
  "libc",
  "redox_syscall",
- "smallvec",
+ "smallvec 1.10.0",
  "windows-sys",
 ]
 
@@ -435,17 +480,20 @@ version = "0.2.16"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "fb5a58c1855b4b6819d59012155603f0b22ad30cad752600aadfcb695265519a"
 dependencies = [
- "bitflags",
+ "bitflags 1.3.2",
 ]
 
 [[package]]
-name = "rustc_version"
-version = "0.4.0"
+name = "regex-automata"
+version = "0.1.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366"
-dependencies = [
- "semver",
-]
+checksum = "6c230d73fb8d8c1b9c0b3135c5142a8acee3a0558fb8db5cf1cb65f8d7862132"
+
+[[package]]
+name = "ryu"
+version = "1.0.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7b4b9743ed687d4b4bcedf9ff5eaa7398495ae14e61cba0a295704edbc7decde"
 
 [[package]]
 name = "satrs-core"
@@ -453,22 +501,37 @@ version = "0.1.0"
 dependencies = [
  "bus",
  "crossbeam-channel",
- "delegate 0.8.0",
+ "delegate",
  "downcast-rs",
  "dyn-clone",
  "embed-doc-image",
  "hashbrown",
- "heapless",
  "num-traits",
  "paste",
+ "serde",
  "spacepackets",
 ]
 
 [[package]]
-name = "scopeguard"
-version = "1.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d29ab0c6d3fc0ee92fe66e2d99f700eab17a8d57d1c1d3b748380fb20baa78cd"
+name = "satrs-mib"
+version = "0.1.0"
+dependencies = [
+ "csv",
+ "satrs-core",
+ "satrs-mib-codegen",
+ "serde",
+ "serde-hex",
+]
+
+[[package]]
+name = "satrs-mib-codegen"
+version = "0.1.0"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "satrs-core",
+ "syn",
+]
 
 [[package]]
 name = "scratch"
@@ -476,12 +539,6 @@ version = "1.0.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9c8132065adcfd6e02db789d9285a0deb2f3fcb04002865ab67d5fb103533898"
 
-[[package]]
-name = "semver"
-version = "1.0.14"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e25dfac463d778e353db5be2449d1cce89bd6fd23c9f1ea21310ce6e5a1b29c4"
-
 [[package]]
 name = "serde"
 version = "1.0.150"
@@ -491,6 +548,17 @@ dependencies = [
  "serde_derive",
 ]
 
+[[package]]
+name = "serde-hex"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ca37e3e4d1b39afd7ff11ee4e947efae85adfddf4841787bfa47c470e96dc26d"
+dependencies = [
+ "array-init",
+ "serde",
+ "smallvec 0.6.14",
+]
+
 [[package]]
 name = "serde_derive"
 version = "1.0.150"
@@ -502,39 +570,48 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "smallvec"
+version = "0.6.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b97fcaeba89edba30f044a10c6a3cc39df9c3f17d7cd829dd1446cab35f890e0"
+dependencies = [
+ "maybe-uninit",
+]
+
 [[package]]
 name = "smallvec"
 version = "1.10.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a507befe795404456341dfab10cef66ead4c041f62b8b11bbb92bffe5d0953e0"
 
+[[package]]
+name = "socketcan"
+version = "1.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3101efc6ef5af6f1c1a488241b469757b7a183baca63af958cd90e4696446c80"
+dependencies = [
+ "hex",
+ "itertools",
+ "libc",
+ "nix",
+ "try_from",
+]
+
 [[package]]
 name = "spacepackets"
-version = "0.2.0"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9199251260fa674673a195bbe68703ba8d7ac0875d97df05cd4b5618218da054"
 dependencies = [
  "chrono",
  "crc",
- "delegate 0.7.0",
+ "delegate",
  "num-traits",
  "serde",
  "zerocopy",
 ]
 
-[[package]]
-name = "spin"
-version = "0.9.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7f6002a767bff9e83f8eeecf883ecb8011875a21ae8da43bffb817a57e78cc09"
-dependencies = [
- "lock_api",
-]
-
-[[package]]
-name = "stable_deref_trait"
-version = "1.2.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
-
 [[package]]
 name = "syn"
 version = "1.0.105"
@@ -555,6 +632,12 @@ dependencies = [
  "winapi-util",
 ]
 
+[[package]]
+name = "try_from"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "923a7ee3e97dbfe8685261beb4511cc9620a1252405d02693d43169729570111"
+
 [[package]]
 name = "unicode-ident"
 version = "1.0.5"
diff --git a/Cargo.toml b/Cargo.toml
index a92c68a..fefd3cd 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -13,11 +13,17 @@ categories = ["aerospace", "hardware-support", "embedded"]
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
+socketcan = {version = "1.7.0", optional = true }
 
-[dependencies.spacepackets]
-path = "../satrs-launchpad/spacepackets"
+#[dependencies.spacepackets]
+#path = "../satrs-launchpad/spacepackets"
 
 [dependencies.satrs-core]
 path = "../satrs-launchpad/satrs-core"
 
+[dependencies.satrs-mib]
+path = "../satrs-launchpad/satrs-mib"
+
 [features]
+default = ["can"]
+can = ["socketcan"]
\ No newline at end of file
diff --git a/pyclient/.gitignore b/pyclient/.gitignore
new file mode 100644
index 0000000..fa91f52
--- /dev/null
+++ b/pyclient/.gitignore
@@ -0,0 +1,6 @@
+/venv
+/log
+/.idea/*
+!/.idea/runConfigurations
+
+/seqcnt.txt
diff --git a/pyclient/main.py b/pyclient/main.py
new file mode 100644
index 0000000..c06f9ed
--- /dev/null
+++ b/pyclient/main.py
@@ -0,0 +1,322 @@
+#!/usr/bin/env python3
+"""Example client for the sat-rs example application"""
+import enum
+import struct
+import sys
+import time
+from typing import Optional
+
+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 tmtccmd import CcsdsTmtcBackend, TcHandlerBase, ProcedureParamsWrapper
+from tmtccmd.tc.pus_3_fsfw_hk import generate_one_hk_command, make_sid
+from tmtccmd.core.base import BackendRequest
+from tmtccmd.pus import VerificationWrapper
+from tmtccmd.tm import CcsdsTmHandler, SpecificApidHandlerBase
+from tmtccmd.com_if import ComInterface
+from tmtccmd.config import (
+    default_json_path,
+    SetupParams,
+    TmTcCfgHookBase,
+    TmtcDefinitionWrapper,
+    CoreServiceList,
+    OpCodeEntry,
+    params_to_procedure_conversion,
+)
+from tmtccmd.config import PreArgsParsingWrapper, SetupWrapper
+from tmtccmd.logging import get_console_logger
+from tmtccmd.logging.pus import (
+    RegularTmtcLogWrapper,
+    RawTmtcTimedLogWrapper,
+    TimedLogWhen,
+)
+from tmtccmd.tc import (
+    TcQueueEntryType,
+    ProcedureWrapper,
+    TcProcedureType,
+    FeedWrapper,
+    SendCbParams,
+    DefaultPusQueueHelper,
+)
+from tmtccmd.tm.pus_5_event import Service5Tm
+from tmtccmd.util import FileSeqCountProvider, PusFileSeqCountProvider
+from tmtccmd.util.obj_id import ObjectIdDictT
+
+from tmtccmd.util.tmtc_printer import FsfwTmTcPrinter
+
+LOGGER = get_console_logger()
+
+EXAMPLE_PUS_APID = 0x02
+
+
+class SatRsConfigHook(TmTcCfgHookBase):
+    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_if 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
+        )
+        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)
+        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(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)
+            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)
+        if service == 17:
+            tm_packet = Service17Tm.unpack(packet)
+            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)
+        self.raw_logger.log_tm(tm_packet)
+        if not dedicated_handler and tm_packet is not None:
+            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
+
+
+class RequestTargetId(enum.IntEnum):
+    ACS = 1
+
+
+class AcsHkIds(enum.IntEnum):
+    MGM_SET = 1
+
+
+class HkOpCodes:
+    GENERATE_ONE_SHOT = ["0", "oneshot"]
+
+
+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=None,
+            seq_cnt_provider=seq_count_provider,
+        )
+
+    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()
+                pus_tc_wrapper.pus_tc.seq_count = (
+                    self.seq_count_provider.get_and_increment()
+                )
+                self.verif_wrapper.add_tc(pus_tc_wrapper.pus_tc)
+                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_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():
+    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()
+    post_args_wrapper = parser_wrapper.parse(hook_obj)
+    params = SetupParams()
+    proc_wrapper = ProcedureParamsWrapper()
+    if post_args_wrapper.use_gui:
+        post_args_wrapper.set_params_without_prompts(params, proc_wrapper)
+    else:
+        post_args_wrapper.set_params_with_prompts(params, 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()
diff --git a/pyclient/requirements.txt b/pyclient/requirements.txt
new file mode 100644
index 0000000..8e9634c
--- /dev/null
+++ b/pyclient/requirements.txt
@@ -0,0 +1 @@
+tmtccmd == 3.0.0
diff --git a/pyclient/tmtc_conf.json b/pyclient/tmtc_conf.json
new file mode 100644
index 0000000..ab02100
--- /dev/null
+++ b/pyclient/tmtc_conf.json
@@ -0,0 +1,6 @@
+{
+    "com_if": "udp",
+    "tcpip_udp_ip_addr": "127.0.0.1",
+    "tcpip_udp_port": 7301,
+    "tcpip_udp_recv_max_size": 1500
+}
\ No newline at end of file
diff --git a/src/can.rs b/src/can.rs
index a4c201e..4583121 100644
--- a/src/can.rs
+++ b/src/can.rs
@@ -1,33 +1,218 @@
-use std::sync::Receiver;
-use std::sync::Sender;
-use satrs_core::pool::StoreAddr;
-#[cfg(feature = "can")]
-use socketcan;
-use crate::tmtc::TmStore;
+#![allow(dead_code)]
+#![allow(unused_variables)]
+#![allow(unused_imports)]
 
-struct CanThreadArgs {
-    rx_can_request: Receiver<HkRequest>,
-    tm_sender: Sender<StoreAddr>,
-    tm_store: TmStore,
+use crate::device_handler::DeviceState;
+
+use crate::can_ids::{
+    can_id_to_package_id, package_id_to_can_id, value_to_package_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 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;
+
+
+pub struct CanRxHandler {
+    interface: &'static str,
+    socket: socket::CanSocket,
+    //frame_id_to_sender_id: HashMap<embedded_can::Id, u32>, // double hash map: frame id -> receiver id -> sender handle
+    can_senders: HashMap<ThreadId, Sender<PackageModel>>,
+    //dismissed_ids: Vec<embedded_can::Id>,
+    package_map: HashMap<PackageId, SenderReceiverThread>,
+    //packet_id_to_sender_id: HashMap<PackageId, ThreadId>,
 }
 
-pub type CollInterval = u32;
-pub enum HkIds {
-    AcsHk = 0
-}
-pub enum HkRequest {
-    OneShot(HkIds),
-    Enable(HkIds, CollInterval),        // periodic
-    Disable(HkIds, CollInterval)
+impl CanRxHandler {
+    pub fn new_socket(
+        interface: &'static str,
+        //frame_id_to_sender_id: HashMap<embedded_can::Id, u32>,
+        can_senders: HashMap<ThreadId, Sender<PackageModel>>,
+        package_map: HashMap<PackageId, SenderReceiverThread>,
+    ) -> Result<CanRxHandler, ()> {
+        let socket = socket::CanSocket::open(&interface);
+        if let Ok(socket) = socket {
+            Ok(CanRxHandler {
+                interface,
+                socket,
+                can_senders,
+                package_map,
+            })
+        } else {
+            Err(())
+        }
+    }
+
+    /*
+    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);
+        }
+        Ok(can_wrapper)
+    }
+    */
+
+    pub fn read_frame(&self) -> io::Result<frame::CanFrame> {
+        let frame = self.socket.read_frame();
+        if let Err(e) = &frame {
+            warn!("CAN bus read error: {}", e);
+        }
+        frame
+    }
+
+    pub fn rx_socket(&self) -> Option<CanFrame> {
+        let frame = self.socket.read_frame().ok()?;
+        info!("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);
+        if let Some(frame_id) = frame_id {
+            if self.package_map.contains_key(&frame_id) {
+                let value = self.package_map.get(&frame_id).unwrap();
+                if value.get_sender() != DeviceId::OBC {
+                    let message_sender = self.can_senders.get(&value.get_thread()).unwrap();
+                    let data = frame.data();
+                    let message =
+                        PackageModel::new(frame_id, data).expect("Error generating message.");
+                    message_sender.send(message).expect(&*format!(
+                        "Failure sending can bus frame to thread{:?}, frame id {:?}",
+                        value.get_thread(),
+                        frame_id
+                    ));
+                }
+            }
+        }
+    }
+
+    /*
+    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(())
+    }
+    */
 }
 
-// impl CanThreadArgs {
-//     fn operation(&self) {
-//
-//     }
-// }
-fn can_thread(canthreadargs: CanThreadArgs) {
-    // handle tc requests
-    // request from devices
-    // send tms
-}
\ No newline at end of file
+pub struct CanTxHandler {
+    interface: &'static str,
+    socket: socket::CanSocket,
+    thread_id: ThreadId,
+    package_map: HashMap<PackageId, SenderReceiverThread>,
+}
+
+impl CanTxHandler {
+    pub fn new_socket(
+        interface: &'static str,
+        thread_id: ThreadId,
+        package_map: HashMap<PackageId, SenderReceiverThread>,
+    ) -> 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,
+                thread_id,
+                package_map,
+            })
+        } else {
+            Err(())
+        }
+    }
+
+    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 value.get_thread() == self.thread_id {
+                    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, mismatched thread id: {:?}",
+                        value.get_thread()
+                    );
+                }
+            } else {
+                warn!(
+                    "Message dismissed, wrong sender id: {:?}",
+                    value.get_sender()
+                );
+            }
+        } else {
+            warn!("Message dismissed, wrong package id: {:?}", package_id);
+        }
+    }
+
+    /*
+    pub fn tx_socket_from_frame(&self, frame: frame::CanFrame) -> io::Result<()> {
+        let frame_id = frame.id();
+        if !self.allowed_frame_ids.contains(&frame_id) {
+            warn!(
+                "Requested frame Id {:?} not allowed for current thread",
+                frame.id()
+            );
+        } else if let Err(e) = self.socket.write_frame(&frame) {
+            warn!("CAN bus write error: {}", e);
+        }
+        return Ok(());
+    }
+
+    pub fn tx_socket_from_data(
+        &self,
+        frame_id: embedded_can::Id,
+        frame_data: &[u8],
+    ) -> io::Result<()> {
+        let frame = frame::CanFrame::new(frame_id, &frame_data).expect(&*format!(
+            "Failure sending can bus frame with id {:?}",
+            frame_id
+        ));
+        self.tx_socket_from_frame(frame)
+    }
+    */
+}
+
+pub fn open_socket(interface: &str) -> Result<socket::CanSocket, errors::CanSocketOpenError> {
+    let socket = socket::CanSocket::open(&interface);
+    return socket;
+}
diff --git a/src/can_ids.rs b/src/can_ids.rs
new file mode 100644
index 0000000..717a6b6
--- /dev/null
+++ b/src/can_ids.rs
@@ -0,0 +1,258 @@
+use embedded_can::{Id, StandardId};
+use log::warn;
+use std::collections::HashMap;
+use std::fs;
+use std::sync::mpsc::Sender;
+use std::thread::Thread;
+
+use crate::device_handler::{DeviceProperties, DeviceState, DeviceType};
+use num_derive::{FromPrimitive, ToPrimitive};
+use num_traits::{FromPrimitive, ToPrimitive};
+use strum::IntoEnumIterator; // 0.17.1
+use strum_macros::EnumIter; // 0.17.1
+
+#[derive(Debug, EnumIter, Eq, Hash, PartialEq, Copy, Clone, FromPrimitive)]
+pub enum ThreadId {
+    AOCSThread,
+    TMThread,
+}
+
+#[derive(Debug, EnumIter, Eq, Hash, PartialEq, Copy, Clone, FromPrimitive)]
+pub enum PackageId {
+    PCDUStatusRequest = 10,
+    DevicePowerOnRequest = 11,
+    DevicePowerOffRequest = 12,
+    DevicePowerStatusRequest = 13,
+    PCDUStatusResponse = 20,
+    DevicePowerStatusResponse = 21,
+    DevicePowerOnRequestConfirmation = 22,
+    DevicePowerOffRequestConfirmation = 23,
+    DevicePowerOnConfirmation = 24,
+    DevicePowerOffConfirmation = 25,
+    AOCSControlMGT1 = 31,
+    AOCSControlMGT2 = 32,
+    AOCSControlMGT3 = 33,
+    AOCSControlRWL1 = 34,
+    AOCSControlRWL2 = 35,
+    AOCSControlRWL3 = 36,
+    AOCSDataMGM1 = 41,
+    AOCSDataMGM2 = 42,
+    AOCSDataMGM3 = 43,
+    AOCSDataSunSensor1 = 44,
+    AOCSDataSunSensor2 = 45,
+    AOCSDataSunSensor3 = 46,
+    AOCSDataStarTracker = 47,
+    HousekeepingRequest = 61,
+    HousekeepingMGM1 = 62,
+    HousekeepingMGM2 = 63,
+    HousekeepingMGM3 = 64,
+    HousekeepingSunSensor1 = 65,
+    HousekeepingSunSensor2 = 66,
+    HousekeepingSunSensor3 = 67,
+    HousekeepingStarTracker = 68,
+    HousekeepingMGT1 = 69,
+    HousekeepingMGT2 = 70,
+    HousekeepingMGT3 = 71,
+    HousekeepingRWL1 = 72,
+    HousekeepingRWL2 = 73,
+    HousekeepingRWL3 = 74,
+    CameraImageRequest = 101,
+    CameraImageRequestConfirmation = 102,
+    CameraImageExecutionStart = 103,
+    CameraImageExectutionEnd = 104,
+}
+
+#[derive(Debug, EnumIter, Eq, Hash, PartialEq, Copy, Clone, FromPrimitive)]
+pub enum DeviceId {
+    OBC = 1,
+    PCDU = 2,
+    MGM1 = 3,
+    MGM2 = 4,
+    MGM3 = 5,
+    SunSensor1 = 6,
+    SunSensor2 = 7,
+    SunSensor3 = 8,
+    StarTracker = 9,
+    MGT1 = 10,
+    MGT2 = 11,
+    MGT3 = 12,
+    RWL1 = 13,
+    RWL2 = 14,
+    RWL3 = 15,
+    Camera = 16,
+    All = 17,
+}
+
+#[derive(Debug)]
+pub struct PackageModel {
+    package_id: PackageId,
+    data: Vec<u8>,
+}
+
+impl PackageModel {
+    pub fn new(package_id: PackageId, data: &[u8]) -> Result<PackageModel, ()> {
+        if data.len() > 8 {
+            warn!("Data exceeds maximum length.");
+            return Err(());
+        }
+        let vec = Vec::from(data);
+        return Ok(PackageModel {
+            package_id,
+            data: vec,
+        });
+    }
+
+    pub fn package_id(&self) -> PackageId {
+        self.package_id
+    }
+
+    pub fn data(&self) -> &Vec<u8> {
+        &self.data
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+pub struct SenderReceiverThread {
+    sender: DeviceId,
+    receiver: DeviceId,
+    thread: ThreadId,
+}
+
+impl SenderReceiverThread {
+    pub fn new(sender: DeviceId, receiver: DeviceId, thread: ThreadId) -> SenderReceiverThread {
+        SenderReceiverThread {
+            sender,
+            receiver,
+            thread,
+        }
+    }
+
+    pub fn get_sender(&self) -> DeviceId {
+        self.sender
+    }
+    pub fn get_receiver(&self) -> DeviceId {
+        self.receiver
+    }
+    pub fn get_thread(&self) -> ThreadId {
+        self.thread
+    }
+}
+
+pub fn load_package_ids() -> HashMap<PackageId, SenderReceiverThread> {
+    let mut package_map: HashMap<PackageId, SenderReceiverThread> = HashMap::new();
+
+    let properties = vec![
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::PCDU, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::PCDU, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::PCDU, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::PCDU, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::PCDU, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::PCDU, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::PCDU, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::PCDU, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::PCDU, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::PCDU, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::MGT1, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::MGT2, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::MGT3, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::RWL1, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::RWL2, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::RWL3, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::MGM1, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::MGM2, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::MGM3, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::SunSensor1, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::SunSensor2, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::SunSensor3, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::StarTracker, DeviceId::OBC, ThreadId::AOCSThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::All, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::MGM1, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::MGM2, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::MGM3, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::SunSensor1, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::SunSensor2, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::SunSensor3, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::StarTracker, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::MGT1, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::MGT2, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::MGT3, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::RWL1, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::RWL2, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::RWL3, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::OBC, DeviceId::Camera, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::Camera, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::Camera, DeviceId::OBC, ThreadId::TMThread),
+        SenderReceiverThread::new(DeviceId::Camera, DeviceId::OBC, ThreadId::TMThread),
+    ];
+
+    let mut i = 0;
+    for id in PackageId::iter() {
+        let value = properties.get(i).unwrap();
+        package_map.insert(id, *value);
+        i += 1;
+    }
+
+    return package_map;
+}
+
+pub fn load_device_ids() {
+    let mut package_map: HashMap<DeviceId, DeviceProperties> = HashMap::new();
+
+    let properties = vec![
+        DeviceProperties::new(DeviceType::OBC, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::PCDU, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::MGM, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::MGM, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::MGM, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::CRS, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::CRS, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::CRS, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::STR, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::MGT, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::MGT, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::MGT, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::RWL, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::RWL, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::RWL, DeviceState::Unknown),
+        DeviceProperties::new(DeviceType::CAM, DeviceState::Unknown),
+    ];
+
+    let mut i = 0;
+    for id in DeviceId::iter() {
+        let value = properties.get(i).unwrap();
+        package_map.insert(id, *value);
+        i += 1;
+    }
+}
+
+//TODO: change ids from u32 to embeddedcan ids
+pub fn package_id_to_value(packageid: &PackageId) -> u32 {
+    *packageid as u32
+}
+pub fn package_id_to_can_id(packageid: &PackageId) -> Id {
+    let x = *packageid as u16;
+    Id::Standard(StandardId::new(x).unwrap())
+}
+
+pub fn device_id_to_value(deviceid: &DeviceId) -> u32 {
+    *deviceid as u32
+}
+
+pub fn value_to_package_id(value: u32) -> Option<PackageId> {
+    let element = FromPrimitive::from_u32(value);
+    return element;
+}
+
+pub fn can_id_to_package_id(value: Id) -> Option<PackageId> {
+    let buf = match value {
+        Id::Standard(id) => id.as_raw() as u32,
+        Id::Extended(id) => id.as_raw(),
+    };
+    let element = FromPrimitive::from_u32(buf);
+    return element;
+}
+
+pub fn value_to_device_id(value: u32) -> Option<DeviceId> {
+    let element = FromPrimitive::from_u32(value);
+    return element;
+}
diff --git a/src/ccsds.rs b/src/ccsds.rs
index cbaf5c7..0ac9533 100644
--- a/src/ccsds.rs
+++ b/src/ccsds.rs
@@ -1,13 +1,13 @@
-use crate::tmtc::PUS_APID;
-use satrs_core::tmtc::{CcsdsPacketHandler, PusDistributor, ReceivesCcsdsTc};
-use spacepackets::{CcsdsPacket, SpHeader};
+use crate::tmtc::{MpscStoreAndSendError, PusTcSource, PUS_APID};
+use satrs_core::tmtc::{CcsdsPacketHandler, ReceivesCcsdsTc};
+use satrs_core::spacepackets::{CcsdsPacket, SpHeader};
 
 pub struct CcsdsReceiver {
-    pub pus_handler: PusDistributor<()>,
+    pub tc_source: PusTcSource,
 }
 
 impl CcsdsPacketHandler for CcsdsReceiver {
-    type Error = ();
+    type Error = MpscStoreAndSendError;
 
     fn valid_apids(&self) -> &'static [u16] {
         &[PUS_APID]
@@ -19,9 +19,7 @@ impl CcsdsPacketHandler for CcsdsReceiver {
         tc_raw: &[u8],
     ) -> Result<(), Self::Error> {
         if sp_header.apid() == PUS_APID {
-            self.pus_handler
-                .pass_ccsds(sp_header, tc_raw)
-                .expect("Handling PUS packet failed");
+            return self.tc_source.pass_ccsds(sp_header, tc_raw);
         }
         Ok(())
     }
diff --git a/src/device_handler.rs b/src/device_handler.rs
new file mode 100644
index 0000000..9631a81
--- /dev/null
+++ b/src/device_handler.rs
@@ -0,0 +1,37 @@
+use socketcan::{errors, frame, socket, CanFrame, Socket};
+use crate::can_ids::DeviceId;
+
+#[derive(Copy, Clone)]
+pub enum DeviceState {
+    On,
+    Off,
+    Broken,
+    Unknown,
+}
+
+#[derive(Copy, Clone)]
+pub enum DeviceType {
+    MGM,
+    MGT,
+    RWL,
+    STR,
+    CRS,
+    OBC,
+    PCDU,
+    CAM,
+}
+
+#[derive(Copy, Clone)]
+pub struct DeviceProperties {
+    device_type: DeviceType,
+    device_state: DeviceState,
+}
+
+impl DeviceProperties {
+    pub fn new(device_type: DeviceType, device_state: DeviceState) -> DeviceProperties {
+        return DeviceProperties {
+            device_type,
+            device_state,
+        };
+    }
+}
diff --git a/src/example_main.rs b/src/example_main.rs
new file mode 100644
index 0000000..d3b7568
--- /dev/null
+++ b/src/example_main.rs
@@ -0,0 +1,243 @@
+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,
+};
+use satrs_core::event_man::{
+    EventManagerWithMpscQueue, MpscEventReceiver, MpscEventU32SendProvider, SendEventProvider,
+};
+use satrs_core::events::EventU32;
+use satrs_core::pool::{LocalPool, PoolCfg, StoreAddr};
+use satrs_core::pus::event_man::{
+    DefaultPusMgmtBackendProvider, EventReporter, EventRequest, EventRequestWithToken,
+    PusEventDispatcher,
+};
+use satrs_core::pus::hk::Subservice;
+use satrs_core::pus::verification::{
+    MpscVerifSender, VerificationReporterCfg, VerificationReporterWithSender,
+};
+use satrs_core::pus::{EcssTmError, EcssTmSenderCore};
+use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProvider};
+use eurosim_obsw::{RequestTargetId, OBSW_SERVER_ADDR, SERVER_PORT};
+use satrs_core::{
+    spacepackets::time::cds::TimeProvider,
+    spacepackets::time::TimeWriter,
+    spacepackets::tm::{PusTm, PusTmSecondaryHeader},
+    spacepackets::{SequenceFlags, SpHeader}
+};
+
+use std::collections::HashMap;
+use std::net::{IpAddr, SocketAddr};
+use std::sync::mpsc::{channel, TryRecvError};
+use std::sync::{mpsc, Arc, RwLock};
+use std::thread;
+use std::time::Duration;
+use crate::EventTmSender;
+
+fn example_main() {
+    let tm_pool = LocalPool::new(PoolCfg::new(vec![
+        (30, 32),
+        (15, 64),
+        (15, 128),
+        (15, 256),
+        (15, 1024),
+        (15, 2048),
+    ]));
+    let tm_store = TmStore {
+        pool: Arc::new(RwLock::new(Box::new(tm_pool))),
+    };
+    let tc_pool = LocalPool::new(PoolCfg::new(vec![
+        (30, 32),
+        (15, 64),
+        (15, 128),
+        (15, 256),
+        (15, 1024),
+        (15, 2048),
+    ]));
+    let tc_store = TcStore {
+        pool: Arc::new(RwLock::new(Box::new(tc_pool))),
+    };
+
+    let sock_addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
+    let (tc_source_tx, tc_source_rx) = channel();
+    let (tm_funnel_tx, tm_funnel_rx) = channel();
+    let (tm_server_tx, tm_server_rx) = channel();
+    let verif_sender = MpscVerifSender::new(tm_store.pool.clone(), tm_funnel_tx.clone());
+    let verif_cfg = VerificationReporterCfg::new(
+        PUS_APID,
+        #[allow(clippy::box_default)]
+            Box::new(SeqCountProviderSyncClonable::default()),
+        1,
+        2,
+        8,
+    )
+        .unwrap();
+    let verif_reporter = VerificationReporterWithSender::new(&verif_cfg, Box::new(verif_sender));
+
+    // Create event handling components
+    let (event_request_tx, event_request_rx) = channel::<EventRequestWithToken>();
+    let (event_sender, event_man_rx) = channel();
+    let event_recv = MpscEventReceiver::<EventU32>::new(event_man_rx);
+    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 =
+        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();
+    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>();
+    request_map.insert(RequestTargetId::AcsSubsystem as u32, acs_thread_tx);
+
+    let tc_source = PusTcSource {
+        tc_store,
+        tc_source: tc_source_tx,
+    };
+
+    // Create clones here to allow moving the values
+    let core_args = OtherArgs {
+        sock_addr,
+        verif_reporter,
+        event_sender,
+        event_request_tx,
+        request_map,
+    };
+    let tc_args = TcArgs {
+        tc_source,
+        tc_receiver: tc_source_rx,
+    };
+    let tm_args = TmArgs {
+        tm_store: tm_store.clone(),
+        tm_sink_sender: tm_funnel_tx.clone(),
+        tm_server_rx,
+    };
+
+    let aocs_to_funnel = tm_funnel_tx.clone();
+    let mut aocs_tm_store = tm_store.clone();
+
+    println!("Starting TMTC task");
+    let jh0 = thread::spawn(move || {
+        core_tmtc_task(core_args, tc_args, tm_args);
+    });
+
+    println!("Starting TM funnel task");
+    let jh1 = thread::spawn(move || {
+        let tm_funnel = TmFunnel {
+            tm_server_tx,
+            tm_funnel_rx,
+        };
+        loop {
+            if let Ok(addr) = tm_funnel.tm_funnel_rx.recv() {
+                tm_funnel
+                    .tm_server_tx
+                    .send(addr)
+                    .expect("Sending TM to server failed");
+            }
+        }
+    });
+
+    println!("Starting event handling task");
+    let jh2 = thread::spawn(move || {
+        let mut timestamp: [u8; 7] = [0; 7];
+        let mut sender = EventTmSender::new(tm_store, tm_funnel_tx);
+        let mut time_provider = TimeProvider::new_with_u16_days(0, 0);
+        let mut report_completion = |event_req: EventRequestWithToken, timestamp: &[u8]| {
+            reporter_event_handler
+                .completion_success(event_req.token, timestamp)
+                .expect("Sending completion success failed");
+        };
+        loop {
+            if let Ok(event_req) = event_request_rx.try_recv() {
+                match event_req.request {
+                    EventRequest::Enable(event) => {
+                        pus_event_dispatcher
+                            .enable_tm_for_event(&event)
+                            .expect("Enabling TM failed");
+                        update_time(&mut time_provider, &mut timestamp);
+                        report_completion(event_req, &timestamp);
+                    }
+                    EventRequest::Disable(event) => {
+                        pus_event_dispatcher
+                            .disable_tm_for_event(&event)
+                            .expect("Disabling TM failed");
+                        update_time(&mut time_provider, &mut timestamp);
+                        report_completion(event_req, &timestamp);
+                    }
+                }
+            }
+            if let Ok((event, _param)) = pus_event_man_rx.try_recv() {
+                update_time(&mut time_provider, &mut timestamp);
+                pus_event_dispatcher
+                    .generate_pus_event_tm_generic(&mut sender, &timestamp, event, None)
+                    .expect("Sending TM as event failed");
+            }
+            thread::sleep(Duration::from_millis(400));
+        }
+    });
+
+    println!("Starting AOCS thread");
+    let jh3 = thread::spawn(move || {
+        let mut timestamp: [u8; 7] = [0; 7];
+        let mut time_provider = TimeProvider::new_with_u16_days(0, 0);
+        loop {
+            match acs_thread_rx.try_recv() {
+                Ok(request) => {
+                    println!("ACS thread: Received HK request {:?}", request.0);
+                    update_time(&mut time_provider, &mut timestamp);
+                    match request.0 {
+                        Request::HkRequest(hk_req) => match hk_req {
+                            HkRequest::OneShot(address) => {
+                                assert_eq!(address.target_id, RequestTargetId::AcsSubsystem as u32);
+                                if address.unique_id == AcsHkIds::TestMgmSet as u32 {
+                                    let mut sp_header =
+                                        SpHeader::tm(PUS_APID, SequenceFlags::Unsegmented, 0, 0)
+                                            .unwrap();
+                                    let sec_header = PusTmSecondaryHeader::new_simple(
+                                        3,
+                                        Subservice::TmHkPacket as u8,
+                                        &timestamp,
+                                    );
+                                    let mut buf: [u8; 8] = [0; 8];
+                                    address.write_to_be_bytes(&mut buf).unwrap();
+                                    let pus_tm =
+                                        PusTm::new(&mut sp_header, sec_header, Some(&buf), true);
+                                    let addr = aocs_tm_store.add_pus_tm(&pus_tm);
+                                    aocs_to_funnel.send(addr).expect("Sending HK TM failed");
+                                }
+                            }
+                            HkRequest::Enable(_) => {}
+                            HkRequest::Disable(_) => {}
+                            HkRequest::ModifyCollectionInterval(_, _) => {}
+                        },
+                    }
+                    let started_token = reporter_aocs
+                        .start_success(request.1, &timestamp)
+                        .expect("Sending start success failed");
+                    reporter_aocs
+                        .completion_success(started_token, &timestamp)
+                        .expect("Sending completion success failed");
+                }
+                Err(e) => match e {
+                    TryRecvError::Empty => {}
+                    TryRecvError::Disconnected => {
+                        println!("ACS thread: Message Queue TX disconnected!")
+                    }
+                },
+            }
+            thread::sleep(Duration::from_millis(500));
+        }
+    });
+}
+
+pub fn update_time(time_provider: &mut TimeProvider, timestamp: &mut [u8]) {
+    time_provider
+        .update_from_now()
+        .expect("Could not get current time");
+    time_provider
+        .write_to_bytes(timestamp)
+        .expect("Writing timestamp failed");
+}
diff --git a/src/hk.rs b/src/hk.rs
new file mode 100644
index 0000000..d27c0f0
--- /dev/null
+++ b/src/hk.rs
@@ -0,0 +1,16 @@
+use satrs_core::tmtc::AddressableId;
+
+pub type CollectionIntervalFactor = u32;
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub enum AcsHkIds {
+    TestMgmSet = 1,
+}
+
+#[derive(Debug, Copy, Clone, PartialEq, Eq)]
+pub enum HkRequest {
+    OneShot(AddressableId),
+    Enable(AddressableId),
+    Disable(AddressableId),
+    ModifyCollectionInterval(AddressableId, CollectionIntervalFactor),
+}
diff --git a/src/lib.rs b/src/lib.rs
index 738bc42..c958d06 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,4 +1,50 @@
 use std::net::Ipv4Addr;
 
-pub const OBSW_SERVER_ADDR: Ipv4Addr = Ipv4Addr::new(0, 0, 0, 0);
+use satrs_mib::res_code::{ResultU16, ResultU16Info};
+use satrs_mib::resultcode;
+
+#[derive(Copy, Clone, Eq, PartialEq, Debug)]
+pub enum RequestTargetId {
+    AcsSubsystem = 1,
+    CanTask = 2,
+}
+
+#[derive(Debug)]
+pub enum GroupId {
+    Tmtc = 0,
+    Hk = 1,
+}
+
+pub const OBSW_SERVER_ADDR: Ipv4Addr = Ipv4Addr::UNSPECIFIED;
 pub const SERVER_PORT: u16 = 7301;
+
+pub mod tmtc_err {
+    use super::*;
+
+    #[resultcode]
+    pub const INVALID_PUS_SERVICE: ResultU16 = ResultU16::const_new(GroupId::Tmtc as u8, 0);
+    #[resultcode]
+    pub const INVALID_PUS_SUBSERVICE: ResultU16 = ResultU16::const_new(GroupId::Tmtc as u8, 1);
+
+    #[resultcode(info = "Not enough data inside the TC application data field")]
+    pub const NOT_ENOUGH_APP_DATA: ResultU16 = ResultU16::const_new(GroupId::Tmtc as u8, 2);
+
+    pub const TMTC_RESULTS: &[ResultU16Info] = &[
+        INVALID_PUS_SERVICE_EXT,
+        INVALID_PUS_SUBSERVICE_EXT,
+        NOT_ENOUGH_APP_DATA_EXT,
+    ];
+}
+
+pub mod hk_err {
+    use super::*;
+
+    #[resultcode]
+    pub const TARGET_ID_MISSING: ResultU16 = ResultU16::const_new(GroupId::Hk as u8, 0);
+    #[resultcode]
+    pub const UNIQUE_ID_MISSING: ResultU16 = ResultU16::const_new(GroupId::Hk as u8, 1);
+    #[resultcode]
+    pub const UNKNOWN_TARGET_ID: ResultU16 = ResultU16::const_new(GroupId::Hk as u8, 2);
+    #[resultcode]
+    pub const COLLECTION_INTERVAL_MISSING: ResultU16 = ResultU16::const_new(GroupId::Hk as u8, 3);
+}
diff --git a/src/logger.rs b/src/logger.rs
new file mode 100644
index 0000000..52d46b0
--- /dev/null
+++ b/src/logger.rs
@@ -0,0 +1,22 @@
+#![allow(unused_imports)]
+
+use chrono;
+use log::{debug, error, info, trace, warn};
+
+pub fn setup_logger() -> Result<(), fern::InitError> {
+    fern::Dispatch::new()
+        .format(|out, message, record| {
+            out.finish(format_args!(
+                "{}[{}][{}] {}",
+                chrono::Local::now().format("[%Y-%m-%d][%H:%M:%S]"),
+                std::thread::current().name().expect("unnamed_thread"),
+                record.level(),
+                message
+            ))
+        })
+        .level(log::LevelFilter::Debug)
+        .chain(std::io::stdout())
+        .chain(fern::log_file("output.log")?)
+        .apply()?;
+    Ok(())
+}
diff --git a/src/main.rs b/src/main.rs
index a0df392..157a6be 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,52 +1,52 @@
-#![allow(dead_code)]
 mod ccsds;
+mod hk;
 mod pus;
+mod requests;
 mod tmtc;
-#[cfg(target_os = "unix")]
+mod example_main;
 mod can;
+mod can_ids;
+mod device_handler;
+mod logger;
 
-
-
-use crate::tmtc::{core_tmtc_task, CoreTmtcArgs, TmStore, PUS_APID};
+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,
+};
 use satrs_core::event_man::{
     EventManagerWithMpscQueue, MpscEventReceiver, MpscEventU32SendProvider, SendEventProvider,
 };
 use satrs_core::events::EventU32;
-use satrs_core::hal::host::udp_server::UdpTcServer;
-use satrs_core::pool::{LocalPool, PoolCfg, SharedPool, StoreAddr};
+use satrs_core::pool::{LocalPool, PoolCfg, StoreAddr};
 use satrs_core::pus::event_man::{
     DefaultPusMgmtBackendProvider, EventReporter, EventRequest, EventRequestWithToken,
     PusEventDispatcher,
 };
+use satrs_core::pus::hk::Subservice;
 use satrs_core::pus::verification::{
     MpscVerifSender, VerificationReporterCfg, VerificationReporterWithSender,
 };
-use satrs_core::pus::{EcssTmError, EcssTmSender};
-use satrs_core::seq_count::SimpleSeqCountProvider;
-use satrs_core::tmtc::CcsdsError;
-use eurosim_obsw::{OBSW_SERVER_ADDR, SERVER_PORT};
-use spacepackets::time::cds::TimeProvider;
-use spacepackets::time::TimeWriter;
-use spacepackets::tm::PusTm;
+use satrs_core::pus::{EcssTmError, EcssTmErrorWithSend, EcssTmSenderCore};
+use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProvider, SequenceCountProviderCore};
+use eurosim_obsw::{RequestTargetId, OBSW_SERVER_ADDR, SERVER_PORT};
+use satrs_core::{
+    spacepackets::time::cds::TimeProvider,
+    spacepackets::time::TimeWriter,
+    spacepackets::tm::{PusTm, PusTmSecondaryHeader},
+    spacepackets::{SequenceFlags, SpHeader},
+};
+
+use std::collections::HashMap;
 use std::net::{IpAddr, SocketAddr};
-use std::sync::mpsc::channel;
+use std::sync::mpsc::{channel, TryRecvError};
 use std::sync::{mpsc, Arc, RwLock};
 use std::thread;
-
-struct TmFunnel {
-    tm_funnel_rx: mpsc::Receiver<StoreAddr>,
-    tm_server_tx: mpsc::Sender<StoreAddr>,
-}
-
-struct UdpTmtcServer {
-    udp_tc_server: UdpTcServer<CcsdsError<()>>,
-    tm_rx: mpsc::Receiver<StoreAddr>,
-    tm_store: SharedPool,
-}
-
-#[cfg(feature = "can")]
-
-unsafe impl Send for UdpTmtcServer {}
+use std::time::Duration;
+use embedded_can::{Id, StandardId};
+use log::{info, warn};
+use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
+use crate::can_ids::{can_id_to_package_id, load_package_ids, PackageId, PackageModel, ThreadId};
 
 #[derive(Clone)]
 struct EventTmSender {
@@ -63,35 +63,63 @@ impl EventTmSender {
     }
 }
 
-impl EcssTmSender for EventTmSender {
+impl EcssTmSenderCore for EventTmSender {
     type Error = mpsc::SendError<StoreAddr>;
 
-    fn send_tm(&mut self, tm: PusTm) -> Result<(), EcssTmError<Self::Error>> {
+    fn send_tm(&mut self, tm: PusTm) -> Result<(), EcssTmErrorWithSend<Self::Error>> {
         let addr = self.store_helper.add_pus_tm(&tm);
-        self.sender.send(addr).map_err(EcssTmError::SendError)
+        self.sender.send(addr).map_err(EcssTmErrorWithSend::SendError)
     }
 }
+
 fn main() {
-    println!("Running OBSW example");
-    let pool_cfg = PoolCfg::new(vec![(8, 32), (4, 64), (2, 128)]);
-    let tm_pool = LocalPool::new(pool_cfg);
-    let tm_store: SharedPool = Arc::new(RwLock::new(Box::new(tm_pool)));
-    let tm_store_helper = TmStore {
-        pool: tm_store.clone(),
+    println!("Running SESPSat OBSW");
+
+    logger::setup_logger().unwrap();
+
+    let tm_pool = LocalPool::new(PoolCfg::new(vec![
+        (30, 32),
+        (15, 64),
+        (15, 128),
+        (15, 256),
+        (15, 1024),
+        (15, 2048),
+    ]));
+    let tm_store = TmStore {
+        pool: Arc::new(RwLock::new(Box::new(tm_pool))),
     };
-    let addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
+    let tc_pool = LocalPool::new(PoolCfg::new(vec![
+        (30, 32),
+        (15, 64),
+        (15, 128),
+        (15, 256),
+        (15, 1024),
+        (15, 2048),
+    ]));
+    let tc_store = TcStore {
+        pool: Arc::new(RwLock::new(Box::new(tc_pool))),
+    };
+
+    let seq_count_provider = SeqCountProviderSyncClonable::default();
+    let aocs_seq_count_provider = seq_count_provider.clone();
+
+    let sock_addr = SocketAddr::new(IpAddr::V4(OBSW_SERVER_ADDR), SERVER_PORT);
+    let (tc_source_tx, tc_source_rx) = channel();
     let (tm_funnel_tx, tm_funnel_rx) = channel();
     let (tm_server_tx, tm_server_rx) = channel();
-    let sender = MpscVerifSender::new(tm_store.clone(), tm_funnel_tx.clone());
+    let verif_sender = MpscVerifSender::new(tm_store.pool.clone(), tm_funnel_tx.clone());
     let verif_cfg = VerificationReporterCfg::new(
         PUS_APID,
-        Box::new(SimpleSeqCountProvider::default()),
+        #[allow(clippy::box_default)]
+            Box::new(seq_count_provider.clone()),
         1,
         2,
         8,
     )
-    .unwrap();
-    let reporter_with_sender_0 = VerificationReporterWithSender::new(&verif_cfg, Box::new(sender));
+        .unwrap();
+
+
+    let verif_reporter = VerificationReporterWithSender::new(&verif_cfg, Box::new(verif_sender));
 
     // Create event handling components
     let (event_request_tx, event_request_rx) = channel::<EventRequestWithToken>();
@@ -104,26 +132,163 @@ fn main() {
         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 reporter1 = reporter_with_sender_0.clone();
+    let mut reporter_event_handler = verif_reporter.clone();
+    let mut reporter_aocs = verif_reporter.clone();
     event_man.subscribe_all(pus_event_man_send_provider.id());
 
-    // Create clones here to allow move for thread 0
-    let core_args = CoreTmtcArgs {
-        tm_store: tm_store_helper.clone(),
-        tm_sender: tm_funnel_tx.clone(),
-        // can_tm_sender: tm_funnel_tx.clone(),
-        // can_tm_store: tm_store_helper.clone(),
-        event_sender,
-        event_request_tx,
+    let mut request_map = HashMap::new();
+    let (acs_thread_tx, acs_thread_rx) = channel::<RequestWithToken>();
+    let (can_thread_tx, can_thread_rx) = channel::<RequestWithToken>();
+    request_map.insert(RequestTargetId::AcsSubsystem as u32, acs_thread_tx);
+    //request_map.insert(RequestTargetId::CanTask as u32, can_thread_tx);
+
+    let tc_source = PusTcSource {
+        tc_store,
+        tc_source: tc_source_tx,
     };
 
-    println!("Starting TMTC task");
+    // Create clones here to allow moving the values
+    let core_args = OtherArgs {
+        sock_addr,
+        verif_reporter,
+        event_sender,
+        event_request_tx,
+        request_map,
+    };
+    let tc_args = TcArgs {
+        tc_source,
+        tc_receiver: tc_source_rx,
+    };
+    let tm_args = TmArgs {
+        tm_store: tm_store.clone(),
+        tm_sink_sender: tm_funnel_tx.clone(),
+        tm_server_rx,
+    };
+
+    let (aocs_can_tx, aocs_can_rx) = mpsc::channel::<PackageModel>();
+    let (tm_can_tx, tm_can_rx) = mpsc::channel::<PackageModel>();
+
+    // make tx thread id hashmap
+    let mut can_senders = HashMap::new();
+    can_senders.insert(ThreadId::AOCSThread, aocs_can_tx);
+    can_senders.insert(ThreadId::TMThread, tm_can_tx);
+
+    // 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");
     let jh0 = thread::spawn(move || {
-        core_tmtc_task(core_args, tm_server_rx, addr, reporter_with_sender_0);
+        core_tmtc_task(core_args, tc_args, tm_args);
     });
 
+    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);
+        }
+    });
+
+    let allowed_ids_range = 101..110;
+    let mut allowed_ids_aocs = Vec::new();
+    for id in allowed_ids_range {
+        allowed_ids_aocs.push(Id::Standard(StandardId::new(id).unwrap()));
+    }
+
+    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();
+
+    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(_) => {}
+            }
+        }
+    }
+);
+
     println!("Starting TM funnel task");
-    let jh1 = thread::spawn(move || {
+    let jh3 = thread::spawn(move || {
         let tm_funnel = TmFunnel {
             tm_server_tx,
             tm_funnel_rx,
@@ -138,59 +303,20 @@ fn main() {
         }
     });
 
-    println!("Starting event handling task");
-    let jh2 = thread::spawn(move || {
-        let mut timestamp: [u8; 7] = [0; 7];
-        let mut sender = EventTmSender::new(tm_store_helper, tm_funnel_tx);
-        let mut time_provider = TimeProvider::new_with_u16_days(0, 0);
-        let mut report_completion = |event_req: EventRequestWithToken, timestamp: &[u8]| {
-            reporter1
-                .completion_success(event_req.token, timestamp)
-                .expect("Sending completion success failed");
-        };
-        loop {
-            if let Ok(event_req) = event_request_rx.try_recv() {
-                match event_req.request {
-                    EventRequest::Enable(event) => {
-                        pus_event_dispatcher
-                            .enable_tm_for_event(&event)
-                            .expect("Enabling TM failed");
-                        update_time(&mut time_provider, &mut timestamp);
-                        report_completion(event_req, &timestamp);
-                    }
-                    EventRequest::Disable(event) => {
-                        pus_event_dispatcher
-                            .disable_tm_for_event(&event)
-                            .expect("Disabling TM failed");
-                        update_time(&mut time_provider, &mut timestamp);
-                        report_completion(event_req, &timestamp);
-                    }
-                }
-            }
-            if let Ok((event, _param)) = pus_event_man_rx.try_recv() {
-                update_time(&mut time_provider, &mut timestamp);
-                pus_event_dispatcher
-                    .generate_pus_event_tm_generic(&mut sender, &timestamp, event, None)
-                    .expect("Sending TM as event failed");
-            }
-        }
-    });
-
-    own_main();
     jh0.join().expect("Joining UDP TMTC server thread failed");
+    jh1.unwrap().join().expect("Joining CAN Bus Listening thread failed");
+    jh2.unwrap().join().expect("Joing AOCS thread failed");
+    jh3.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");
+    */
 }
-
-pub fn update_time(time_provider: &mut TimeProvider, timestamp: &mut [u8]) {
-    time_provider
-        .update_from_now()
-        .expect("Could not get current time");
-    time_provider
-        .write_to_bytes(timestamp)
-        .expect("Writing timestamp failed");
-}
-
-fn own_main() {
-
-}
+#[derive(Default)]
+struct MgmData {
+    x: i16,
+    y: i16,
+    z: i16
+}
\ No newline at end of file
diff --git a/src/pus.rs b/src/pus.rs
index 73146fa..8ac77b6 100644
--- a/src/pus.rs
+++ b/src/pus.rs
@@ -1,26 +1,38 @@
-use crate::tmtc::TmStore;
+use crate::hk::{CollectionIntervalFactor, HkRequest};
+use crate::requests::{Request, RequestWithToken};
+use crate::tmtc::{PusTcSource, TmStore};
 use satrs_core::events::EventU32;
 use satrs_core::pool::StoreAddr;
 use satrs_core::pus::event::Subservices;
 use satrs_core::pus::event_man::{EventRequest, EventRequestWithToken};
+use satrs_core::pus::hk;
 use satrs_core::pus::verification::{
     FailParams, StdVerifReporterWithSender, TcStateAccepted, VerificationToken,
 };
+use satrs_core::res_code::ResultU16;
 use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
-use satrs_core::tmtc::PusServiceProvider;
-use spacepackets::ecss::{EcssEnumU16, PusPacket};
-use spacepackets::tc::PusTc;
-use spacepackets::time::cds::TimeProvider;
-use spacepackets::time::TimeWriter;
-use spacepackets::SpHeader;
-use std::sync::mpsc;
+use satrs_core::tmtc::{AddressableId, PusServiceProvider};
+use eurosim_obsw::{hk_err, tmtc_err};
+use satrs_core::{
+    spacepackets::ecss::PusPacket,
+    spacepackets::tc::PusTc,
+    spacepackets::time::cds::TimeProvider,
+    spacepackets::time::TimeWriter,
+    spacepackets::SpHeader,
+};
+
+use std::collections::HashMap;
+use std::sync::mpsc::Sender;
 
 pub struct PusReceiver {
     pub tm_helper: PusTmWithCdsShortHelper,
-    pub tm_tx: mpsc::Sender<StoreAddr>,
+    pub tm_tx: Sender<StoreAddr>,
     pub tm_store: TmStore,
     pub verif_reporter: StdVerifReporterWithSender,
-    event_request_tx: mpsc::Sender<EventRequestWithToken>,
+    #[allow(dead_code)]
+    tc_source: PusTcSource,
+    event_request_tx: Sender<EventRequestWithToken>,
+    request_map: HashMap<u32, Sender<RequestWithToken>>,
     stamper: TimeProvider,
     time_stamp: [u8; 7],
 }
@@ -28,18 +40,22 @@ pub struct PusReceiver {
 impl PusReceiver {
     pub fn new(
         apid: u16,
-        tm_tx: mpsc::Sender<StoreAddr>,
+        tm_tx: Sender<StoreAddr>,
         tm_store: TmStore,
         verif_reporter: StdVerifReporterWithSender,
-        event_request_tx: mpsc::Sender<EventRequestWithToken>,
+        tc_source: PusTcSource,
+        event_request_tx: Sender<EventRequestWithToken>,
+        request_map: HashMap<u32, Sender<RequestWithToken>>,
     ) -> Self {
         Self {
             tm_helper: PusTmWithCdsShortHelper::new(apid),
             tm_tx,
             tm_store,
             verif_reporter,
+            tc_source,
             event_request_tx,
-            stamper: TimeProvider::default(),
+            request_map,
+            stamper: TimeProvider::new_with_u16_days(0, 0),
             time_stamp: [0; 7],
         }
     }
@@ -63,27 +79,25 @@ impl PusServiceProvider for PusReceiver {
         if service == 17 {
             self.handle_test_service(pus_tc, accepted_token);
         } else if service == 5 {
-            self.handle_event_service(pus_tc, accepted_token);
+            self.handle_event_request(pus_tc, accepted_token);
+        } else if service == 3 {
+            self.handle_hk_request(pus_tc, accepted_token);
         } else {
-            // TODO: Unknown service verification failure
-            // TODO: Unknown service returncode
+            self.update_time_stamp();
+            self.verif_reporter
+                .start_failure(
+                    accepted_token,
+                    FailParams::new(&self.time_stamp, &tmtc_err::INVALID_PUS_SERVICE, None),
+                )
+                .expect("Start failure verification failed")
         }
         Ok(())
     }
 }
 
-pub type CollInterval = u32;
-pub enum HkIds {
-    AcsHk = 0
-}
-pub enum HkRequest {
-    OneShot(HkIds),
-    Enable(HkIds, CollInterval),        // periodic
-    Disable(HkIds, CollInterval)
-}
 impl PusReceiver {
     fn handle_test_service(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
-        if pus_tc.subservice() == 1 {
+        if PusPacket::subservice(pus_tc) == 1 {
             println!("Received PUS ping command TC[17,1]");
             println!("Sending ping reply PUS TM[17,2]");
             let ping_reply = self.tm_helper.create_pus_tm_timestamp_now(17, 2, None);
@@ -99,12 +113,11 @@ impl PusReceiver {
                 .completion_success(start_token, &self.time_stamp)
                 .expect("Error sending completion success");
         } else {
-            // TODO: Unknown Subservice returncode
             self.update_time_stamp();
             self.verif_reporter
                 .start_failure(
                     token,
-                    FailParams::new(&self.time_stamp, &EcssEnumU16::new(2), None),
+                    FailParams::new(&self.time_stamp, &tmtc_err::INVALID_PUS_SUBSERVICE, None),
                 )
                 .expect("Sending start failure TM failed");
         }
@@ -119,15 +132,84 @@ impl PusReceiver {
             .expect("Writing timestamp failed");
     }
 
-    fn handle_event_service(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
+    fn handle_hk_request(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
+        if pus_tc.user_data().is_none() {
+            self.update_time_stamp();
+            self.verif_reporter
+                .start_failure(
+                    token,
+                    FailParams::new(&self.time_stamp, &tmtc_err::NOT_ENOUGH_APP_DATA, None),
+                )
+                .expect("Sending start failure TM failed");
+            return;
+        }
+        let user_data = pus_tc.user_data().unwrap();
+        if user_data.len() < 8 {
+            let err = if user_data.len() < 4 {
+                &hk_err::TARGET_ID_MISSING
+            } else {
+                &hk_err::UNIQUE_ID_MISSING
+            };
+            self.update_time_stamp();
+            self.verif_reporter
+                .start_failure(token, FailParams::new(&self.time_stamp, err, None))
+                .expect("Sending start failure TM failed");
+            return;
+        }
+        let addressable_id = AddressableId::from_raw_be(user_data).unwrap();
+        if !self.request_map.contains_key(&addressable_id.target_id) {
+            self.update_time_stamp();
+            self.verif_reporter
+                .start_failure(
+                    token,
+                    FailParams::new(&self.time_stamp, &hk_err::UNKNOWN_TARGET_ID, None),
+                )
+                .expect("Sending start failure TM failed");
+            return;
+        }
+        let send_request = |request: HkRequest| {
+            let sender = self.request_map.get(&addressable_id.target_id).unwrap();
+            sender
+                .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 {
+            send_request(HkRequest::Enable(addressable_id));
+        } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcDisableGeneration 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
+        {
+            if user_data.len() < 12 {
+                self.update_time_stamp();
+                self.verif_reporter
+                    .start_failure(
+                        token,
+                        FailParams::new(
+                            &self.time_stamp,
+                            &hk_err::COLLECTION_INTERVAL_MISSING,
+                            None,
+                        ),
+                    )
+                    .expect("Sending start failure TM failed");
+                return;
+            }
+            send_request(HkRequest::ModifyCollectionInterval(
+                addressable_id,
+                CollectionIntervalFactor::from_be_bytes(user_data[8..12].try_into().unwrap()),
+            ));
+        }
+    }
+    fn handle_event_request(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
         let send_start_failure = |verif_reporter: &mut StdVerifReporterWithSender,
                                   timestamp: &[u8; 7],
-                                  failure_code: EcssEnumU16,
+                                  failure_code: &ResultU16,
                                   failure_data: Option<&[u8]>| {
             verif_reporter
                 .start_failure(
                     token,
-                    FailParams::new(timestamp, &failure_code, failure_data),
+                    FailParams::new(timestamp, failure_code, failure_data),
                 )
                 .expect("Sending start failure TM failed");
         };
@@ -142,7 +224,7 @@ impl PusReceiver {
             send_start_failure(
                 &mut self.verif_reporter,
                 &self.time_stamp,
-                EcssEnumU16::new(1),
+                &tmtc_err::NOT_ENOUGH_APP_DATA,
                 None,
             );
             return;
@@ -153,7 +235,7 @@ impl PusReceiver {
             send_start_failure(
                 &mut self.verif_reporter,
                 &self.time_stamp,
-                EcssEnumU16::new(1),
+                &tmtc_err::NOT_ENOUGH_APP_DATA,
                 None,
             );
             return;
@@ -181,12 +263,11 @@ impl PusReceiver {
                     .expect("Sending event request failed");
             }
             _ => {
-                // TODO: Unknown Subservice returncode
                 self.update_time_stamp();
                 send_start_failure(
                     &mut self.verif_reporter,
                     &self.time_stamp,
-                    EcssEnumU16::new(2),
+                    &tmtc_err::INVALID_PUS_SUBSERVICE,
                     None,
                 );
             }
diff --git a/src/requests.rs b/src/requests.rs
new file mode 100644
index 0000000..cac2277
--- /dev/null
+++ b/src/requests.rs
@@ -0,0 +1,10 @@
+use crate::hk::HkRequest;
+use satrs_core::pus::verification::{TcStateAccepted, VerificationToken};
+
+#[derive(Copy, Clone, Eq, PartialEq, Debug)]
+pub enum Request {
+    HkRequest(HkRequest),
+}
+
+#[derive(Copy, Clone, Eq, PartialEq, Debug)]
+pub struct RequestWithToken(pub Request, pub VerificationToken<TcStateAccepted>);
diff --git a/src/tmtc.rs b/src/tmtc.rs
index 03d0db0..2da5408 100644
--- a/src/tmtc.rs
+++ b/src/tmtc.rs
@@ -1,28 +1,91 @@
 use satrs_core::events::EventU32;
 use satrs_core::hal::host::udp_server::{ReceiveResult, UdpTcServer};
 use satrs_core::params::Params;
+use std::collections::HashMap;
+use std::error::Error;
+use std::fmt::{Display, Formatter};
 use std::net::SocketAddr;
-use std::sync::mpsc;
-use std::sync::mpsc::Sender;
+use std::sync::mpsc::{Receiver, SendError, Sender, TryRecvError};
 use std::thread;
 use std::time::Duration;
 
 use crate::ccsds::CcsdsReceiver;
 use crate::pus::PusReceiver;
-use crate::UdpTmtcServer;
-use satrs_core::pool::{SharedPool, StoreAddr};
+use crate::requests::RequestWithToken;
+use satrs_core::pool::{SharedPool, StoreAddr, StoreError};
 use satrs_core::pus::event_man::EventRequestWithToken;
 use satrs_core::pus::verification::StdVerifReporterWithSender;
-use satrs_core::tmtc::{CcsdsDistributor, CcsdsError, PusDistributor};
-use spacepackets::tm::PusTm;
+use satrs_core::tmtc::{
+    CcsdsDistributor, CcsdsError, PusServiceProvider, ReceivesCcsdsTc, ReceivesEcssPusTc,
+};
+
+use satrs_core::{
+    spacepackets::ecss::PusPacket,
+    spacepackets::tc::PusTc,
+    spacepackets::tm::PusTm,
+    spacepackets::SpHeader,
+};
+
 
 pub const PUS_APID: u16 = 0x02;
 
-pub struct CoreTmtcArgs {
-    pub tm_store: TmStore,
-    pub tm_sender: Sender<StoreAddr>,
+pub struct OtherArgs {
+    pub sock_addr: SocketAddr,
+    pub verif_reporter: StdVerifReporterWithSender,
     pub event_sender: Sender<(EventU32, Option<Params>)>,
     pub event_request_tx: Sender<EventRequestWithToken>,
+    pub request_map: HashMap<u32, Sender<RequestWithToken>>,
+}
+
+pub struct TmArgs {
+    pub tm_store: TmStore,
+    pub tm_sink_sender: Sender<StoreAddr>,
+    pub tm_server_rx: Receiver<StoreAddr>,
+}
+
+pub struct TcArgs {
+    pub tc_source: PusTcSource,
+    pub tc_receiver: Receiver<StoreAddr>,
+}
+
+#[derive(Debug, Clone, PartialEq, Eq)]
+pub enum MpscStoreAndSendError {
+    StoreError(StoreError),
+    SendError(SendError<StoreAddr>),
+}
+
+impl Display for MpscStoreAndSendError {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        match self {
+            MpscStoreAndSendError::StoreError(s) => {
+                write!(f, "store error {}", s)
+            }
+            MpscStoreAndSendError::SendError(s) => {
+                write!(f, "send error {}", s)
+            }
+        }
+    }
+}
+
+impl Error for MpscStoreAndSendError {
+    fn source(&self) -> Option<&(dyn Error + 'static)> {
+        match self {
+            MpscStoreAndSendError::StoreError(s) => Some(s),
+            MpscStoreAndSendError::SendError(s) => Some(s),
+        }
+    }
+}
+
+impl From<StoreError> for MpscStoreAndSendError {
+    fn from(value: StoreError) -> Self {
+        Self::StoreError(value)
+    }
+}
+
+impl From<SendError<StoreAddr>> for MpscStoreAndSendError {
+    fn from(value: SendError<StoreAddr>) -> Self {
+        Self::SendError(value)
+    }
 }
 
 #[derive(Clone)]
@@ -30,57 +93,138 @@ pub struct TmStore {
     pub pool: SharedPool,
 }
 
+#[derive(Clone)]
+pub struct TcStore {
+    pub pool: SharedPool,
+}
+
 impl TmStore {
     pub fn add_pus_tm(&mut self, pus_tm: &PusTm) -> StoreAddr {
-        let mut pg = self.pool.write().expect("Error locking TM store");
+        let mut pg = self.pool.write().expect("error locking TM store");
         let (addr, buf) = pg.free_element(pus_tm.len_packed()).expect("Store error");
         pus_tm
             .write_to_bytes(buf)
-            .expect("Writing PUS TM to store failed");
+            .expect("writing PUS TM to store failed");
         addr
     }
 }
 
-pub fn core_tmtc_task(
-    args: CoreTmtcArgs,
-    tm_server_rx: mpsc::Receiver<StoreAddr>,
-    addr: SocketAddr,
-    verif_reporter: StdVerifReporterWithSender,
-) {
-    let pus_receiver = PusReceiver::new(
+impl TcStore {
+    pub fn add_pus_tc(&mut self, pus_tc: &PusTc) -> Result<StoreAddr, StoreError> {
+        let mut pg = self.pool.write().expect("error locking TC store");
+        let (addr, buf) = pg.free_element(pus_tc.len_packed())?;
+        pus_tc
+            .write_to_bytes(buf)
+            .expect("writing PUS TC to store failed");
+        Ok(addr)
+    }
+}
+
+pub struct TmFunnel {
+    pub tm_funnel_rx: Receiver<StoreAddr>,
+    pub tm_server_tx: Sender<StoreAddr>,
+}
+
+pub struct UdpTmtcServer {
+    udp_tc_server: UdpTcServer<CcsdsError<MpscStoreAndSendError>>,
+    tm_rx: Receiver<StoreAddr>,
+    tm_store: SharedPool,
+}
+
+#[derive(Clone)]
+pub struct PusTcSource {
+    pub tc_source: Sender<StoreAddr>,
+    pub tc_store: TcStore,
+}
+
+impl ReceivesEcssPusTc for PusTcSource {
+    type Error = MpscStoreAndSendError;
+
+    fn pass_pus_tc(&mut self, _: &SpHeader, pus_tc: &PusTc) -> Result<(), Self::Error> {
+        let addr = self.tc_store.add_pus_tc(pus_tc)?;
+        self.tc_source.send(addr)?;
+        Ok(())
+    }
+}
+
+impl ReceivesCcsdsTc for PusTcSource {
+    type Error = MpscStoreAndSendError;
+
+    fn pass_ccsds(&mut self, _: &SpHeader, tc_raw: &[u8]) -> Result<(), Self::Error> {
+        let mut pool = self.tc_store.pool.write().expect("locking pool failed");
+        let addr = pool.add(tc_raw)?;
+        drop(pool);
+        self.tc_source.send(addr)?;
+        Ok(())
+    }
+}
+pub fn core_tmtc_task(args: OtherArgs, mut tc_args: TcArgs, tm_args: TmArgs) {
+    let mut pus_receiver = PusReceiver::new(
         PUS_APID,
-        args.tm_sender,
-        args.tm_store.clone(),
-        verif_reporter,
+        tm_args.tm_sink_sender,
+        tm_args.tm_store.clone(),
+        args.verif_reporter,
+        tc_args.tc_source.clone(),
         args.event_request_tx,
+        args.request_map,
     );
-    let pus_distributor = PusDistributor::new(Box::new(pus_receiver));
     let ccsds_receiver = CcsdsReceiver {
-        pus_handler: pus_distributor,
+        tc_source: tc_args.tc_source.clone(),
     };
     let ccsds_distributor = CcsdsDistributor::new(Box::new(ccsds_receiver));
-    let udp_tc_server = UdpTcServer::new(addr, 2048, Box::new(ccsds_distributor))
+    let udp_tc_server = UdpTcServer::new(args.sock_addr, 2048, Box::new(ccsds_distributor))
         .expect("Creating UDP TMTC server failed");
 
     let mut udp_tmtc_server = UdpTmtcServer {
         udp_tc_server,
-        tm_rx: tm_server_rx,
-        tm_store: args.tm_store.pool.clone(),
+        tm_rx: tm_args.tm_server_rx,
+        tm_store: tm_args.tm_store.pool.clone(),
     };
     loop {
-        core_tmtc_loop(&mut udp_tmtc_server);
+        core_tmtc_loop(&mut udp_tmtc_server, &mut tc_args, &mut pus_receiver);
         thread::sleep(Duration::from_millis(400));
     }
 }
 
-fn core_tmtc_loop(udp_tmtc_server: &mut UdpTmtcServer) {
-    while core_tc_handling(udp_tmtc_server) {}
+fn core_tmtc_loop(
+    udp_tmtc_server: &mut UdpTmtcServer,
+    tc_args: &mut TcArgs,
+    pus_receiver: &mut PusReceiver,
+) {
+    while poll_tc_server(udp_tmtc_server) {}
+    match tc_args.tc_receiver.try_recv() {
+        Ok(addr) => {
+            let pool = tc_args
+                .tc_source
+                .tc_store
+                .pool
+                .read()
+                .expect("locking tc pool failed");
+            let data = pool.read(&addr).expect("reading pool failed");
+            match PusTc::from_bytes(data) {
+                Ok((pus_tc, _)) => {
+                    pus_receiver
+                        .handle_pus_tc_packet(pus_tc.service(), pus_tc.sp_header(), &pus_tc)
+                        .ok();
+                }
+                Err(e) => {
+                    println!("error creating PUS TC from raw data: {}", e);
+                    println!("raw data: {:x?}", data);
+                }
+            }
+        }
+        Err(e) => {
+            if let TryRecvError::Disconnected = e {
+                println!("tmtc thread: sender disconnected")
+            }
+        }
+    }
     if let Some(recv_addr) = udp_tmtc_server.udp_tc_server.last_sender() {
         core_tm_handling(udp_tmtc_server, &recv_addr);
     }
 }
 
-fn core_tc_handling(udp_tmtc_server: &mut UdpTmtcServer) -> bool {
+fn poll_tc_server(udp_tmtc_server: &mut UdpTmtcServer) -> bool {
     match udp_tmtc_server.udp_tc_server.try_recv_tc() {
         Ok(_) => true,
         Err(e) => match e {
@@ -108,9 +252,9 @@ fn core_tm_handling(udp_tmtc_server: &mut UdpTmtcServer, recv_addr: &SocketAddr)
         let mut store_lock = udp_tmtc_server
             .tm_store
             .write()
-            .expect("Locking TM store failed");
+            .expect("locking TM store failed");
         let pg = store_lock.read_with_guard(addr);
-        let buf = pg.read().expect("Error reading TM pool data");
+        let buf = pg.read().expect("error reading TM pool data");
         if buf.len() > 9 {
             let service = buf[7];
             let subservice = buf[8];
@@ -122,6 +266,6 @@ fn core_tm_handling(udp_tmtc_server: &mut UdpTmtcServer, recv_addr: &SocketAddr)
             .udp_tc_server
             .socket
             .send_to(buf, recv_addr)
-            .expect("Sending TM failed");
+            .expect("sending TM failed");
     }
 }