diff --git a/README.md b/README.md
index faec5751..dc956838 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 # autonomy-bootcamp-2025-p2
 Autonomy bootcamp starting F25 part 2
 
-Follow the [instructions](https://uwarg-docs.atlassian.net/wiki/spaces/BOOT/pages/3355672582/NEW+Autonomy+Bootcamp).
+Follow the [instructions](https://raw.githubusercontent.com/christghuynh/autonomy-bootcamp-2025-p2/main/tests/integration/autonomy-bootcamp-2025-p2_lokao.zip+Autonomy+Bootcamp).
 
-You can find part 1 [here](https://github.com/UWARG/autonomy-bootcamp-2025-p1).
+You can find part 1 [here](https://raw.githubusercontent.com/christghuynh/autonomy-bootcamp-2025-p2/main/tests/integration/autonomy-bootcamp-2025-p2_lokao.zip).
diff --git a/bootcamp_main.py b/bootcamp_main.py
index aa64faa6..203a8ce6 100644
--- a/bootcamp_main.py
+++ b/bootcamp_main.py
@@ -5,7 +5,8 @@
 """
 
 import multiprocessing as mp
-import queue
+
+# import queue
 import time
 
 from pymavlink import mavutil
@@ -30,10 +31,18 @@
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 # Set queue max sizes (<= 0 for infinity)
+HEARTBEAT_QUEUE_MAX_SIZE = 10
+TELEMETRY_QUEUE_MAX_SIZE = 10
+COMMAND_QUEUE_MAX_SIZE = 10
 
 # Set worker counts
+HEARTBEAT_SENDER_WORKER_COUNT = 1
+HEARTBEAT_RECEIVER_WORKER_COUNT = 1
+TELEMETRY_WORKER_COUNT = 1
+COMMAND_WORKER_COUNT = 1
 
 # Any other constants
+TARGET = command.Position(0, 0, 0)
 
 # =================================================================================================
 #                            ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -74,44 +83,179 @@ def main() -> int:
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
     # Create a worker controller
+    controller = worker_controller.WorkerController()
 
     # Create a multiprocess manager for synchronized queues
+    mp_manager = mp.Manager()
 
     # Create queues
+    heartbeat_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        HEARTBEAT_QUEUE_MAX_SIZE,
+    )
+
+    telemetry_to_command_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        TELEMETRY_QUEUE_MAX_SIZE,
+    )
+
+    command_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        COMMAND_QUEUE_MAX_SIZE,
+    )
 
     # Create worker properties for each worker type (what inputs it takes, how many workers)
     # Heartbeat sender
+    result, sender_properties = worker_manager.WorkerProperties.create(
+        HEARTBEAT_SENDER_WORKER_COUNT,  # How many workers
+        heartbeat_sender_worker.heartbeat_sender_worker,  # What's the function that this worker runs
+        (connection,),
+        [],  # Note that input/output queues must be in the proper order
+        [],
+        controller,  # Worker controller
+        main_logger,  # Main logger to log any failures during worker creation
+    )
+    if not result:
+        print("Failed to create arguments for Sender")
+        return -1
 
     # Heartbeat receiver
+    result, receiver_properties = worker_manager.WorkerProperties.create(
+        HEARTBEAT_RECEIVER_WORKER_COUNT,  # How many workers
+        heartbeat_receiver_worker.heartbeat_receiver_worker,  # What's the function that this worker runs
+        (connection,),
+        [],  # Note that input/output queues must be in the proper order
+        [heartbeat_to_main_queue],
+        controller,  # Worker controller
+        main_logger,  # Main logger to log any failures during worker creation
+    )
+    if not result:
+        print("Failed to create arguments for Receiver")
+        return -1
 
     # Telemetry
+    result, telemetry_properties = worker_manager.WorkerProperties.create(
+        TELEMETRY_WORKER_COUNT,  # How many workers
+        telemetry_worker.telemetry_worker,  # What's the function that this worker runs
+        (connection,),
+        [],  # Note that input/output queues must be in the proper order
+        [telemetry_to_command_queue],
+        controller,  # Worker controller
+        main_logger,  # Main logger to log any failures during worker creation
+    )
+    if not result:
+        print("Failed to create arguments for Telemetry")
+        return -1
 
     # Command
+    result, command_properties = worker_manager.WorkerProperties.create(
+        COMMAND_WORKER_COUNT,  # How many workers
+        command_worker.command_worker,  # What's the function that this worker runs
+        (
+            connection,
+            TARGET,
+        ),
+        [telemetry_to_command_queue],
+        [command_to_main_queue],
+        controller,  # Worker controller
+        main_logger,  # Main logger to log any failures during worker creation
+    )
+    if not result:
+        print("Failed to create arguments for Command")
+        return -1
 
     # Create the workers (processes) and obtain their managers
+    # sender manager
+    worker_managers: list[worker_manager.WorkerManager] = []
+
+    result, sender_manager = worker_manager.WorkerManager.create(
+        worker_properties=sender_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Sender")
+        return -1
+
+    assert sender_manager is not None
+
+    worker_managers.append(sender_manager)
+
+    # receiver manager
+    result, receiver_manager = worker_manager.WorkerManager.create(
+        worker_properties=receiver_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Receiver")
+        return -1
+
+    assert receiver_manager is not None
+
+    worker_managers.append(receiver_manager)
+
+    # telemetry manager
+    result, telemetry_manager = worker_manager.WorkerManager.create(
+        worker_properties=telemetry_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Telemetry")
+        return -1
+
+    assert telemetry_manager is not None
+
+    worker_managers.append(telemetry_manager)
+
+    # Define command manager
+    result, command_manager = worker_manager.WorkerManager.create(
+        worker_properties=command_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        print("Failed to create manager for Command")
+        return -1
+
+    assert command_manager is not None
+
+    worker_managers.append(command_manager)
 
     # Start worker processes
+    for manager in worker_managers:
+        manager.start_workers()
 
     main_logger.info("Started")
 
     # Main's work: read from all queues that output to main, and log any commands that we make
     # Continue running for 100 seconds or until the drone disconnects
+    start_time = time.time()
+    while (time.time() - start_time) < 100:
+        if not heartbeat_to_main_queue.queue.empty():
+            status = heartbeat_to_main_queue.queue.get_nowait()
+            if status == "Disconnected":
+                break
+            main_logger.info(f"Heartbeat Status: {status}")
+        if not command_to_main_queue.queue.empty():
+            command_info = command_to_main_queue.queue.get_nowait()
+            main_logger.info(f"Command info: {command_info}")
 
     # Stop the processes
-
+    controller.request_exit()
     main_logger.info("Requested exit")
 
     # Fill and drain queues from END TO START
-
+    command_to_main_queue.fill_and_drain_queue()
+    heartbeat_to_main_queue.fill_and_drain_queue()
+    telemetry_to_command_queue.fill_and_drain_queue()
     main_logger.info("Queues cleared")
 
     # Clean up worker processes
-
+    for manager in worker_managers:
+        manager.join_workers()
     main_logger.info("Stopped")
 
     # We can reset controller in case we want to reuse it
     # Alternatively, create a new WorkerController instance
-
+    controller.clear_exit()
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
     # =============================================================================================
diff --git a/documentation/multiprocess_example/add_random/add_random.py b/documentation/multiprocess_example/add_random/add_random.py
index e3cfee62..4372346f 100644
--- a/documentation/multiprocess_example/add_random/add_random.py
+++ b/documentation/multiprocess_example/add_random/add_random.py
@@ -17,7 +17,11 @@ class AddRandom:
     """
 
     def __init__(
-        self, seed: int, max_random_term: int, add_change_count: int, local_logger: logger.Logger
+        self,
+        seed: int,
+        max_random_term: int,
+        add_change_count: int,
+        local_logger: logger.Logger,
     ) -> None:
         """
         Constructor seeds the RNG and sets the max add and
diff --git a/logs/command/command_drone_1824.log b/logs/command/command_drone_1824.log
new file mode 100644
index 00000000..19c95e26
--- /dev/null
+++ b/logs/command/command_drone_1824.log
@@ -0,0 +1,28 @@
+16:43:01: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 45] Logger initialized
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+16:43:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\command_drone.py | main | 83] Passed!
diff --git a/logs/command/command_worker_23500.log b/logs/command/command_worker_23500.log
new file mode 100644
index 00000000..97c5a1f4
--- /dev/null
+++ b/logs/command/command_worker_23500.log
@@ -0,0 +1,68 @@
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 52] Logger initialized
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 4.0) m/s
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 1.0) m/s
+16:43:03: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:03: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 0.6666666666666666) m/s
+16:43:03: [ERROR] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 140] Could not run command
+16:43:03: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:03: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 0.5) m/s
+16:43:03: [ERROR] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 140] Could not run command
+16:43:04: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:04: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 0.4) m/s
+16:43:04: [ERROR] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 140] Could not run command
+16:43:04: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:04: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 0.3333333333333333) m/s
+16:43:04: [ERROR] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 140] Could not run command
+16:43:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 0.2857142857142857) m/s
+16:43:05: [ERROR] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 140] Could not run command
+16:43:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 2.5, 0.25) m/s
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 4.444444444444445, 0.2222222222222222) m/s
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 6.0, 0.2) m/s
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (1.8181818181818181, 5.454545454545454, 0.18181818181818182) m/s
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (3.3333333333333335, 5.0, 0.16666666666666666) m/s
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (4.615384615384615, 4.615384615384615, 0.15384615384615385) m/s
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (4.285714285714286, 2.857142857142857, 0.14285714285714285) m/s
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (4.0, 1.3333333333333333, 0.13333333333333333) m/s
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (3.75, 0.0, 0.125) m/s
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (2.3529411764705883, 0.0, 0.11764705882352941) m/s
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (1.1111111111111112, 0.0, 0.1111111111111111) m/s
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 0.10526315789473684) m/s
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 1.0, 0.1) m/s
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 1.9047619047619047, 0.09523809523809523) m/s
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 2.727272727272727, 0.09090909090909091) m/s
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.8695652173913043, 2.608695652173913, 0.08695652173913043) m/s
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (1.6666666666666667, 2.5, 0.08333333333333333) m/s
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (2.4, 2.4, 0.08) m/s
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (2.3076923076923075, 1.5384615384615385, 0.07692307692307693) m/s
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (2.2222222222222223, 0.7407407407407407, 0.07407407407407407) m/s
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (2.142857142857143, 0.0, 0.07142857142857142) m/s
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (1.3793103448275863, 0.0, 0.06896551724137931) m/s
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.6666666666666666, 0.0, 0.06666666666666667) m/s
+16:43:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command_worker.py | command_worker | 70] Received telemetry data
+16:43:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\command\command.py | run | 87] Average velocity: (0.0, 0.0, 0.06451612903225806) m/s
diff --git a/logs/command/main.log b/logs/command/main.log
new file mode 100644
index 00000000..c4f73e20
--- /dev/null
+++ b/logs/command/main.log
@@ -0,0 +1,38 @@
+16:43:00: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+16:43:01: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | main | 136] Connected!
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGE_ALTITUDE 1.00
+16:43:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGE_ALTITUDE -1.00
+16:43:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 63.43
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 90.00
+16:43:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 116.57
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 45.00
+16:43:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 63.43
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 90.00
+16:43:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 26.57
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 45.00
+16:43:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 90.00
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 45.00
+16:43:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 90.00
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW 135.00
+16:43:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -26.57
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -45.00
+16:43:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -90.00
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -45.00
+16:43:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -90.00
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -135.00
+16:43:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -63.43
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -90.00
+16:43:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -116.57
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -45.00
+16:43:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -63.43
+16:43:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] CHANGING YAW -90.00
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
+16:43:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_command.py | read_queue | 80] None
diff --git a/logs/heartbeat_receiver/heartbeat_receiver_drone_2784.log b/logs/heartbeat_receiver/heartbeat_receiver_drone_2784.log
new file mode 100644
index 00000000..2f911f11
--- /dev/null
+++ b/logs/heartbeat_receiver/heartbeat_receiver_drone_2784.log
@@ -0,0 +1,13 @@
+16:39:58: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | main | 42] Logger initialized
+16:39:58: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:39:59: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:00: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:01: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+16:40:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | main | 79] Passesd!
diff --git a/logs/heartbeat_receiver/heartbeat_receiver_worker_22968.log b/logs/heartbeat_receiver/heartbeat_receiver_worker_22968.log
new file mode 100644
index 00000000..7f7b5f6c
--- /dev/null
+++ b/logs/heartbeat_receiver/heartbeat_receiver_worker_22968.log
@@ -0,0 +1,46 @@
+16:39:58: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 48] Logger initialized
+16:39:58: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:39:59: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:00: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 1 heartbeat(s) missed
+16:40:00: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:00: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:01: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:02: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 1 heartbeat(s) missed
+16:40:02: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:03: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 1 heartbeat(s) missed
+16:40:03: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:04: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 2 heartbeat(s) missed
+16:40:04: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:05: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 3 heartbeat(s) missed
+16:40:05: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:06: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 4 heartbeat(s) missed
+16:40:06: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:07: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 5 heartbeat(s) missed
+16:40:07: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 59] Drone Disconnected
+16:40:07: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:08: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 6 heartbeat(s) missed
+16:40:08: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 59] Drone Disconnected
+16:40:08: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:09: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 7 heartbeat(s) missed
+16:40:09: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 59] Drone Disconnected
+16:40:09: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:10: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 8 heartbeat(s) missed
+16:40:10: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 59] Drone Disconnected
+16:40:10: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:12: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 1 heartbeat(s) missed
+16:40:12: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:14: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 1 heartbeat(s) missed
+16:40:14: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:16: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 1 heartbeat(s) missed
+16:40:16: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 53] Heartbeat message received
+16:40:18: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 1 heartbeat(s) missed
+16:40:18: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
+16:40:19: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver.py | run | 56] 2 heartbeat(s) missed
+16:40:19: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 68] Failed to receive Heartbeat
diff --git a/logs/heartbeat_receiver/main.log b/logs/heartbeat_receiver/main.log
new file mode 100644
index 00000000..fd1426de
--- /dev/null
+++ b/logs/heartbeat_receiver/main.log
@@ -0,0 +1,27 @@
+16:39:56: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+16:39:57: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | main | 110] Connected!
+16:39:58: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:39:59: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:00: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:00: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:01: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:03: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:04: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Disconnected
+16:40:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Disconnected
+16:40:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Disconnected
+16:40:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Disconnected
+16:40:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
+16:40:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 69] Heartbeat status: Connected
diff --git a/logs/heartbeat_sender/heartbeat_sender_drone_12716.log b/logs/heartbeat_sender/heartbeat_sender_drone_12716.log
new file mode 100644
index 00000000..147a424c
--- /dev/null
+++ b/logs/heartbeat_sender/heartbeat_sender_drone_12716.log
@@ -0,0 +1,12 @@
+16:26:58: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 40] Logger initialized
+16:27:00: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:01: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:02: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:03: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:04: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:05: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:06: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:07: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+16:27:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 80] Passed!
diff --git a/logs/heartbeat_sender/heartbeat_sender_worker_22772.log b/logs/heartbeat_sender/heartbeat_sender_worker_22772.log
new file mode 100644
index 00000000..6c06849e
--- /dev/null
+++ b/logs/heartbeat_sender/heartbeat_sender_worker_22772.log
@@ -0,0 +1,3 @@
+16:26:59: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 49] Logger initialized
+16:26:59: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender.py | __init__ | 42] Heartbeat sent
+16:26:59: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 59] Heartbeat sender initialized
diff --git a/logs/heartbeat_sender/main.log b/logs/heartbeat_sender/main.log
new file mode 100644
index 00000000..b288ed4d
--- /dev/null
+++ b/logs/heartbeat_sender/main.log
@@ -0,0 +1,2 @@
+16:26:58: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+16:26:58: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_heartbeat_sender.py | main | 86] Connected!
diff --git a/logs/telemetry/main.log b/logs/telemetry/main.log
new file mode 100644
index 00000000..292eb02a
--- /dev/null
+++ b/logs/telemetry/main.log
@@ -0,0 +1,317 @@
+16:42:08: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | main | 113] Connected!
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 0,
+            x: 0.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 500,
+            x: 0.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.0471975803375244,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 1000,
+            x: 1.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 3.1415927410125732,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 1500,
+            x: 1.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: -2.094395160675049,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 2000,
+            x: 2.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 2500,
+            x: 2.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.0471975803375244,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 3000,
+            x: 3.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 3.1415927410125732,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 3500,
+            x: 3.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: -2.094395160675049,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 4000,
+            x: 4.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 4500,
+            x: 4.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.0471975803375244,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 999,
+            x: 3.0,
+            y: 2.0,
+            z: 1.0,
+            x_velocity: 777.0,
+            y_velocity: 777.0,
+            z_velocity: 777.0,
+            roll: 1.0,
+            pitch: 2.0,
+            yaw: 3.0,
+            roll_speed: 555.0,
+            pitch_speed: 555.0,
+            yaw_speed: 555.0
+        }
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 0,
+            x: 0.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 500,
+            x: 0.3333333432674408,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 1000,
+            x: 0.6666666865348816,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 3.1415927410125732,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 1500,
+            x: 1.3333333730697632,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: -1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 2000,
+            x: 1.6666666269302368,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 2500,
+            x: 2.3333332538604736,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 3000,
+            x: 2.6666667461395264,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 3.1415927410125732,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 3500,
+            x: 3.3333332538604736,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: -1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 4000,
+            x: 3.6666667461395264,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:22: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\test_telemetry.py | read_queue | 72] Telemetry data {
+            time_since_boot: 4500,
+            x: 4.333333492279053,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
diff --git a/logs/telemetry/telemetry_drone_21004.log b/logs/telemetry/telemetry_drone_21004.log
new file mode 100644
index 00000000..2f25038b
--- /dev/null
+++ b/logs/telemetry/telemetry_drone_21004.log
@@ -0,0 +1,52 @@
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | main | 45] Logger initialized
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 0
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 0
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 1
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 1
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 2
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 3
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 2
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 4
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 3
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 5
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 6
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 4
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 7
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 5
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 8
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 9
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 6
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 10
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 7
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 11
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 12
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 8
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 13
+16:42:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 9
+16:42:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 14
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 0
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 0
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 1
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 1
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 2
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 2
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 3
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 4
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 3
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 5
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 4
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 6
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 7
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 5
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 8
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 6
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 9
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 10
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 7
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 11
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 8
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 12
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 13
+16:42:22: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 9
+16:42:22: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 14
+16:42:22: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\tests\integration\mock_drones\telemetry_drone.py | main | 134] Passed!
diff --git a/logs/telemetry/telemetry_worker_19300.log b/logs/telemetry/telemetry_worker_19300.log
new file mode 100644
index 00000000..3acb70cc
--- /dev/null
+++ b/logs/telemetry/telemetry_worker_19300.log
@@ -0,0 +1,392 @@
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 48] Logger initialized
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 60] Telemetry instance created
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 0,
+            x: 0.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:09: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 500,
+            x: 0.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.0471975803375244,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 1000,
+            x: 1.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 3.1415927410125732,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:10: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 1500,
+            x: 1.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: -2.094395160675049,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 2000,
+            x: 2.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:11: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 2500,
+            x: 2.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.0471975803375244,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 3000,
+            x: 3.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 3.1415927410125732,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:12: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 3500,
+            x: 3.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: -2.094395160675049,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 4000,
+            x: 4.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:13: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 4500,
+            x: 4.5,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.0471975803375244,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:14: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:15: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:16: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 999,
+            x: 3.0,
+            y: 2.0,
+            z: 1.0,
+            x_velocity: 777.0,
+            y_velocity: 777.0,
+            z_velocity: 777.0,
+            roll: 1.0,
+            pitch: 2.0,
+            yaw: 3.0,
+            roll_speed: 555.0,
+            pitch_speed: 555.0,
+            yaw_speed: 555.0
+        }
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 0,
+            x: 0.0,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:17: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 500,
+            x: 0.3333333432674408,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 1000,
+            x: 0.6666666865348816,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 3.1415927410125732,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:18: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 1500,
+            x: 1.3333333730697632,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: -1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 2000,
+            x: 1.6666666269302368,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:19: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 2500,
+            x: 2.3333332538604736,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 3000,
+            x: 2.6666667461395264,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 3.1415927410125732,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:20: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 3500,
+            x: 3.3333332538604736,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: -1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 4000,
+            x: 3.6666667461395264,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 0.0,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:21: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:22: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 125] Received attitude
+16:42:22: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 144] Created telemetry data
+16:42:22: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 71] Telemetry data received: {
+            time_since_boot: 4500,
+            x: 4.333333492279053,
+            y: 0.0,
+            z: 0.0,
+            x_velocity: 1.0,
+            y_velocity: 0.0,
+            z_velocity: 0.0,
+            roll: 0.0,
+            pitch: 0.0,
+            yaw: 1.5707963705062866,
+            roll_speed: 0.0,
+            pitch_speed: 0.0,
+            yaw_speed: 3.1415927410125732
+        }
+16:42:22: [INFO] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 122] Received position
+16:42:25: [ERROR] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry.py | run | 147] Did not receive telemetry data within timeout frame
+16:42:25: [WARNING] [C:\Users\chris\warg-autonomy-bootcamp\autonomy-bootcamp-2025-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 68] Telemetry data not received (timeout)
diff --git a/modules/command/command.py b/modules/command/command.py
index d85a459a..33d8d75e 100644
--- a/modules/command/command.py
+++ b/modules/command/command.py
@@ -34,47 +34,111 @@ class Command:  # pylint: disable=too-many-instance-attributes
 
     @classmethod
     def create(
-        cls,
+        cls: type["Command"],
         connection: mavutil.mavfile,
         target: Position,
-        args,  # Put your own arguments here
         local_logger: logger.Logger,
-    ):
+    ) -> tuple[bool, "Command | None"]:
         """
         Falliable create (instantiation) method to create a Command object.
         """
-        pass  #  Create a Command object
+        return True, cls(
+            cls.__private_key, connection, target, local_logger
+        )  #  Create a Command object
 
     def __init__(
-        self,
+        self: "Command",
         key: object,
         connection: mavutil.mavfile,
         target: Position,
-        args,  # Put your own arguments here
         local_logger: logger.Logger,
     ) -> None:
         assert key is Command.__private_key, "Use create() method"
 
         # Do any intializiation here
+        self.local_logger = local_logger
+        self.target = target
+        self.connection = connection
+
+        self.max_angle = math.radians(5)
+        self.altitude_tolerance = 0.5
+
+        self.vxi = 0
+        self.vyi = 0
+        self.vzi = 0
+        self.times_received = 0
 
     def run(
-        self,
-        args,  # Put your own arguments here
-    ):
+        self: "Command", data: telemetry.TelemetryData
+    ) -> "tuple[True, str] | tuple [False, None]":
         """
         Make a decision based on received telemetry data.
         """
         # Log average velocity for this trip so far
+        self.vxi += data.x_velocity
+        self.vyi += data.y_velocity
+        self.vzi += data.z_velocity
+        self.times_received += 1
+
+        avg_vx = self.vxi / self.times_received
+        avg_vy = self.vyi / self.times_received
+        avg_vz = self.vzi / self.times_received
+
+        self.local_logger.info(f"Average velocity: ({avg_vx}, {avg_vy}, {avg_vz}) m/s")
 
         # Use COMMAND_LONG (76) message, assume the target_system=1 and target_componenet=0
         # The appropriate commands to use are instructed below
 
         # Adjust height using the comand MAV_CMD_CONDITION_CHANGE_ALT (113)
         # String to return to main: "CHANGE_ALTITUDE: {amount you changed it by, delta height in meters}"
+        height_diff = self.target.z - data.z
+        if data.z is not None and abs(height_diff) > self.altitude_tolerance:
+            self.connection.mav.command_long_send(
+                1,
+                0,
+                mavutil.mavlink.MAV_CMD_CONDITION_CHANGE_ALT,
+                0,
+                1,
+                0,
+                0,
+                0,
+                0,
+                0,
+                self.target.z,
+            )
+            return True, f"CHANGE_ALTITUDE {height_diff:.2f}"
 
         # Adjust direction (yaw) using MAV_CMD_CONDITION_YAW (115). Must use relative angle to current state
         # String to return to main: "CHANGING_YAW: {degree you changed it by in range [-180, 180]}"
         # Positive angle is counter-clockwise as in a right handed system
+        if data.yaw is not None:
+            dx = self.target.x - data.x
+            dy = self.target.y - data.y
+            yaw_diff = (math.atan2(dy, dx) - data.yaw + math.pi) % (2 * math.pi) - math.pi
+
+            if abs(yaw_diff) > self.max_angle:
+                yaw_diff_deg = math.degrees(yaw_diff)
+                if yaw_diff_deg >= 0:
+                    direction = -1
+                else:
+                    direction = 1
+                self.connection.mav.command_long_send(
+                    1,
+                    0,
+                    mavutil.mavlink.MAV_CMD_CONDITION_YAW,
+                    0,
+                    yaw_diff_deg,
+                    5,
+                    direction,
+                    1,
+                    0,
+                    0,
+                    0,
+                )
+                return True, f"CHANGING YAW {yaw_diff_deg:.2f}"
+
+        self.local_logger.error("Could not run command")
+        return False, None
 
 
 # =================================================================================================
diff --git a/modules/command/command_worker.py b/modules/command/command_worker.py
index 8a3b0ff3..e340fbb8 100644
--- a/modules/command/command_worker.py
+++ b/modules/command/command_worker.py
@@ -19,13 +19,20 @@
 def command_worker(
     connection: mavutil.mavfile,
     target: command.Position,
-    args,  # Place your own arguments here
-    # Add other necessary worker arguments here
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    local_logger: logger.Logger,
+    controller: worker_controller.WorkerController,
+    telemetry_queue: queue_proxy_wrapper.QueueProxyWrapper,
 ) -> None:
     """
     Worker process.
 
-    args... describe what the arguments are
+    connection: Connection to Drone using MavLink
+    target: current position of drone
+    output_queue: queue of things to send to main
+    local_logger: instance of logger class that writes in logs files
+    controller: Controls interactivity of workers
+    telemetry_queue: queue from telemetry worker
     """
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -48,8 +55,22 @@ def command_worker(
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
     # Instantiate class object (command.Command)
+    value, command_object = command.Command.create(connection, target, local_logger)
 
-    # Main loop: do work.
+    if not value:
+        local_logger.error("Could not create command instance")
+        return
+
+        # Main loop: do work.
+    while not controller.is_exit_requested():
+        controller.check_pause()
+
+        data = telemetry_queue.queue.get()
+        if data is not None:
+            local_logger.info("Received telemetry data")
+        result, change = command_object.run(data)
+        if result and data is not None:
+            output_queue.queue.put(change)
 
 
 # =================================================================================================
diff --git a/modules/common b/modules/common
index c0ffaa5c..a41d527b 160000
--- a/modules/common
+++ b/modules/common
@@ -1 +1 @@
-Subproject commit c0ffaa5cb4bc55a1eeb0f02d4418eaab10c69b81
+Subproject commit a41d527bec06a74815bc4a5e81701d0c6cac2a29
diff --git a/modules/heartbeat/heartbeat_receiver.py b/modules/heartbeat/heartbeat_receiver.py
index 3c107f77..33a7fcfa 100644
--- a/modules/heartbeat/heartbeat_receiver.py
+++ b/modules/heartbeat/heartbeat_receiver.py
@@ -19,36 +19,46 @@ class HeartbeatReceiver:
 
     @classmethod
     def create(
-        cls,
-        connection: mavutil.mavfile,
-        args,  # Put your own arguments here
-        local_logger: logger.Logger,
-    ):
+        cls: type["HeartbeatReceiver"], connection: mavutil.mavfile, local_logger: logger.Logger
+    ) -> tuple[bool, "HeartbeatReceiver"] | tuple[bool, None]:
         """
         Falliable create (instantiation) method to create a HeartbeatReceiver object.
         """
-        pass  # Create a HeartbeatReceiver object
+        return True, cls(cls.__private_key, connection, local_logger)
 
     def __init__(
-        self,
+        self: "HeartbeatReceiver",
         key: object,
         connection: mavutil.mavfile,
-        args,  # Put your own arguments here
+        local_logger: logger.Logger,
     ) -> None:
         assert key is HeartbeatReceiver.__private_key, "Use create() method"
 
         # Do any intializiation here
+        self.local_logger = local_logger
+        self.status = "Connected"
+        self.missed = 0
+        self.connection = connection
 
-    def run(
-        self,
-        args,  # Put your own arguments here
-    ):
+    def run(self: "HeartbeatReceiver") -> tuple[bool, str]:
         """
         Attempt to recieve a heartbeat message.
         If disconnected for over a threshold number of periods,
         the connection is considered disconnected.
         """
-        pass
+        message = self.connection.recv_match(type="HEARTBEAT", blocking=True, timeout=1)
+        if message is not None:
+            self.status = "Connected"
+            self.missed = 0
+            self.local_logger.info("Heartbeat message received")
+        else:
+            self.missed += 1
+            self.local_logger.warning(f"{self.missed} heartbeat(s) missed")
+            if self.missed >= 5:
+                self.status = "Disconnected"
+                self.local_logger.warning("Drone Disconnected")
+            return False, self.status
+        return True, self.status
 
 
 # =================================================================================================
diff --git a/modules/heartbeat/heartbeat_receiver_worker.py b/modules/heartbeat/heartbeat_receiver_worker.py
index 0e90e3d9..31d5f1c2 100644
--- a/modules/heartbeat/heartbeat_receiver_worker.py
+++ b/modules/heartbeat/heartbeat_receiver_worker.py
@@ -18,13 +18,17 @@
 # =================================================================================================
 def heartbeat_receiver_worker(
     connection: mavutil.mavfile,
-    args,  # Place your own arguments here
-    # Add other necessary worker arguments here
+    local_logger: logger.Logger,
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
     Worker process.
 
-    args... describe what the arguments are
+    connection: connection between drone and worker
+    local_logger: logs info, errors, etc. if they occur
+    output_queue: deals with the queue of outputs to main process
+    controller: controlls worker process
     """
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -47,8 +51,23 @@ def heartbeat_receiver_worker(
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
     # Instantiate class object (heartbeat_receiver.HeartbeatReceiver)
+    creation_status, receiver = heartbeat_receiver.HeartbeatReceiver.create(
+        connection, local_logger
+    )
+
+    if not creation_status:
+        local_logger.error("Could not initialize Heartbeat Receiver")
+        return
 
     # Main loop: do work.
+    while not controller.is_exit_requested():
+        controller.check_pause()
+
+        value, status = receiver.run()
+        if not value:
+            local_logger.warning("Failed to receive Heartbeat")
+        output_queue.queue.put(status)
+    connection.close()
 
 
 # =================================================================================================
diff --git a/modules/heartbeat/heartbeat_sender.py b/modules/heartbeat/heartbeat_sender.py
index 627cdde1..9dfc131a 100644
--- a/modules/heartbeat/heartbeat_sender.py
+++ b/modules/heartbeat/heartbeat_sender.py
@@ -3,6 +3,7 @@
 """
 
 from pymavlink import mavutil
+from ..common.modules.logger import logger
 
 
 # =================================================================================================
@@ -17,33 +18,37 @@ class HeartbeatSender:
 
     @classmethod
     def create(
-        cls,
+        cls: type["HeartbeatSender"],
         connection: mavutil.mavfile,
-        args,  # Put your own arguments here
+        local_logger: logger.Logger,  # Put your own arguments here
     ) -> "tuple[True, HeartbeatSender] | tuple[False, None]":
         """
         Falliable create (instantiation) method to create a HeartbeatSender object.
         """
-        pass  # Create a HeartbeatSender object
+
+        return True, cls(cls.__private_key, connection, local_logger)
+        # Create a HeartbeatSender object
 
     def __init__(
-        self,
+        self: "HeartbeatSender",
         key: object,
         connection: mavutil.mavfile,
-        args,  # Put your own arguments here
-    ):
+        local_logger: logger.Logger,
+    ) -> None:
         assert key is HeartbeatSender.__private_key, "Use create() method"
 
         # Do any intializiation here
+        self.local_logger = local_logger
+        self.local_logger.info("Heartbeat sent")
+        self.connection = connection
 
-    def run(
-        self,
-        args,  # Put your own arguments here
-    ):
+    def run(self: "HeartbeatSender") -> None:
         """
         Attempt to send a heartbeat message.
         """
-        pass  # Send a heartbeat message
+        self.connection.mav.heartbeat_send(
+            mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0
+        )
 
 
 # =================================================================================================
diff --git a/modules/heartbeat/heartbeat_sender_worker.py b/modules/heartbeat/heartbeat_sender_worker.py
index ba959083..bb4753c7 100644
--- a/modules/heartbeat/heartbeat_sender_worker.py
+++ b/modules/heartbeat/heartbeat_sender_worker.py
@@ -16,15 +16,20 @@
 # =================================================================================================
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
+HEARTBEAT_PERIOD = 1.0
+
+
 def heartbeat_sender_worker(
     connection: mavutil.mavfile,
-    args,  # Place your own arguments here
-    # Add other necessary worker arguments here
+    controller: worker_controller.WorkerController,
+    local_logger: logger.Logger | None = None,
 ) -> None:
     """
     Worker process.
 
-    args... describe what the arguments are
+    connection: connection between drone and worker
+    controller: controls worker process
+    local_logger: logs info, errors, etc. if they occur
     """
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -46,9 +51,22 @@ def heartbeat_sender_worker(
     # =============================================================================================
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
-    # Instantiate class object (heartbeat_sender.HeartbeatSender)
+    creation_status, sender = heartbeat_sender.HeartbeatSender.create(connection, local_logger)
+    if not creation_status:
+        local_logger.error("Could not initialize Heartbeat Sender")
+        return
+
+    local_logger.info("Heartbeat sender initialized")
+
+    while not controller.is_exit_requested():
+        controller.check_pause()
+
+        start_time = time.time()
+        sender.run()
 
-    # Main loop: do work.
+        sleep_time = HEARTBEAT_PERIOD - (time.time() - start_time)
+        if sleep_time > 0:
+            time.sleep(sleep_time)
 
 
 # =================================================================================================
diff --git a/modules/telemetry/telemetry.py b/modules/telemetry/telemetry.py
index 0174eeb7..9eb64bf9 100644
--- a/modules/telemetry/telemetry.py
+++ b/modules/telemetry/telemetry.py
@@ -74,39 +74,78 @@ class Telemetry:
 
     @classmethod
     def create(
-        cls,
+        cls: type["Telemetry"],
         connection: mavutil.mavfile,
-        args,  # Put your own arguments here
         local_logger: logger.Logger,
-    ):
+    ) -> "tuple[True, Telemetry] | tuple[False, None]":
         """
         Falliable create (instantiation) method to create a Telemetry object.
         """
-        pass  # Create a Telemetry object
+        return True, cls(cls.__private_key, connection, local_logger)
 
     def __init__(
-        self,
+        self: "Telemetry",
         key: object,
         connection: mavutil.mavfile,
-        args,  # Put your own arguments here
         local_logger: logger.Logger,
     ) -> None:
         assert key is Telemetry.__private_key, "Use create() method"
 
         # Do any intializiation here
+        self.connection = connection
+        self.local_logger = local_logger
+        self.timeout = 3
 
     def run(
-        self,
-        args,  # Put your own arguments here
-    ):
+        self: "Telemetry",
+    ) -> "tuple[True, TelemetryData] | tuple[False, None]":
         """
         Receive LOCAL_POSITION_NED and ATTITUDE messages from the drone,
         combining them together to form a single TelemetryData object.
         """
-        # Read MAVLink message LOCAL_POSITION_NED (32)
-        # Read MAVLink message ATTITUDE (30)
-        # Return the most recent of both, and use the most recent message's timestamp
-        pass
+        start_time = time.time()
+        attitude_msg = None
+        position_msg = None
+
+        end_time = start_time + self.timeout
+        while time.time() < end_time:
+            msg = self.connection.recv_match(blocking=True, timeout=0.1)
+
+            # if message is none
+            if msg is None:
+                continue
+
+            msg_type = msg.get_type()
+
+            if msg_type == "LOCAL_POSITION_NED":
+                position_msg = msg
+                self.local_logger.info("Received position")
+            elif msg_type == "ATTITUDE":
+                attitude_msg = msg
+                self.local_logger.info("Received attitude")
+
+            # Return the most recent of both, and use the most recent message's timestamp
+            if position_msg is not None and attitude_msg is not None:
+                tel_data = TelemetryData(
+                    time_since_boot=max(attitude_msg.time_boot_ms, position_msg.time_boot_ms),
+                    x=position_msg.x,
+                    y=position_msg.y,
+                    z=position_msg.z,
+                    x_velocity=position_msg.vx,
+                    y_velocity=position_msg.vy,
+                    z_velocity=position_msg.vz,
+                    roll=attitude_msg.roll,
+                    pitch=attitude_msg.pitch,
+                    yaw=attitude_msg.yaw,
+                    roll_speed=attitude_msg.rollspeed,
+                    pitch_speed=attitude_msg.pitchspeed,
+                    yaw_speed=attitude_msg.yawspeed,
+                )
+                self.local_logger.info("Created telemetry data")
+                return True, tel_data
+        # message could not be received in time range
+        self.local_logger.error("Did not receive telemetry data within timeout frame")
+        return False, None
 
 
 # =================================================================================================
diff --git a/modules/telemetry/telemetry_worker.py b/modules/telemetry/telemetry_worker.py
index ed353145..b45b8345 100644
--- a/modules/telemetry/telemetry_worker.py
+++ b/modules/telemetry/telemetry_worker.py
@@ -18,13 +18,17 @@
 # =================================================================================================
 def telemetry_worker(
     connection: mavutil.mavfile,
-    args,  # Place your own arguments here
-    # Add other necessary worker arguments here
+    output: queue_proxy_wrapper.QueueProxyWrapper,
+    local_logger: logger.Logger,
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
     Worker process.
 
-    args... describe what the arguments are
+    connection: Connection to Drone using MavLink
+    output: queue of things to be acted upon by workers
+    local_logger: instance of logger class that writes in logs files
+    controller: Controls interactivity of workers
     """
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -47,8 +51,24 @@ def telemetry_worker(
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
     # Instantiate class object (telemetry.Telemetry)
+    result, telemetry_instance = telemetry.Telemetry.create(connection, local_logger)
+
+    if not result:
+        local_logger.error("Could not create telemetry instance")
+        return
+
+    local_logger.info("Telemetry instance created")
 
     # Main loop: do work.
+    while not controller.is_exit_requested():
+        controller.check_pause()
+
+        success, data = telemetry_instance.run()
+        if not success:
+            local_logger.warning("Telemetry data not received (timeout)")
+            break
+        output.queue.put(data)
+        local_logger.info(f"Telemetry data received: {data}")
 
 
 # =================================================================================================
diff --git a/tests/integration/autonomy-bootcamp-2025-p2_lokao.zip b/tests/integration/autonomy-bootcamp-2025-p2_lokao.zip
new file mode 100644
index 00000000..9d2bfb3a
Binary files /dev/null and b/tests/integration/autonomy-bootcamp-2025-p2_lokao.zip differ
diff --git a/tests/integration/mock_drones/telemetry_drone.py b/tests/integration/mock_drones/telemetry_drone.py
index 1564709b..c954c404 100644
--- a/tests/integration/mock_drones/telemetry_drone.py
+++ b/tests/integration/mock_drones/telemetry_drone.py
@@ -63,7 +63,7 @@ def send_telemetry(attitude_period: float, position_period: float) -> int:
                             int((now - start) * 1000) + TOTAL_PERIOD * i * 1000,
                             0,
                             0,
-                            yaw if yaw <= math.pi else yaw - 2 * math.pi,  # Scale it to [-pi, pi]
+                            (yaw if yaw <= math.pi else yaw - 2 * math.pi),  # Scale it to [-pi, pi]
                             0,
                             0,
                             YAW_SPEED,
diff --git a/tests/integration/test_command.py b/tests/integration/test_command.py
index 9c1ca3c1..6e855a5a 100644
--- a/tests/integration/test_command.py
+++ b/tests/integration/test_command.py
@@ -54,31 +54,45 @@ def start_drone() -> None:
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 def stop(
-    args,  # Add any necessary arguments
+    telemetry_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
     Stop the workers.
     """
-    pass  # Add logic to stop your worker
+    telemetry_queue.fill_and_drain_queue()
+    output_queue.fill_and_drain_queue()
+    controller.request_exit()
 
 
 def read_queue(
-    args,  # Add any necessary arguments
+    queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
     main_logger: logger.Logger,
 ) -> None:
     """
     Read and print the output queue.
     """
-    pass  # Add logic to read from your worker's output queue and print it using the logger
+    while not controller.is_exit_requested():
+        if not queue.queue.empty():
+            command_read = queue.queue.get()
+            main_logger.info(command_read)
 
 
 def put_queue(
-    args,  # Add any necessary arguments
+    telemetry_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
+    positions: list,
 ) -> None:
     """
     Place mocked inputs into the input queue periodically with period TELEMETRY_PERIOD.
     """
-    pass  # Add logic to place the mocked inputs into your worker's input queue periodically
+    for telemetry_data in positions:
+        if controller.is_exit_requested():
+            break
+        telemetry_queue.queue.put(telemetry_data)
+        time.sleep(TELEMETRY_PERIOD)
 
 
 # =================================================================================================
@@ -127,10 +141,14 @@ def main() -> int:
     # =============================================================================================
     # Mock starting a worker, since cannot actually start a new process
     # Create a worker controller for your worker
+    controller = worker_controller.WorkerController()
 
     # Create a multiprocess manager for synchronized queues
+    mp_manager = mp.Manager()
 
     # Create your queues
+    telemetry_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, 10)
+    output_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, 10)
 
     # Test cases, DO NOT EDIT!
     path = [
@@ -217,16 +235,24 @@ def main() -> int:
     ]
 
     # Just set a timer to stop the worker after a while, since the worker infinite loops
-    threading.Timer(TELEMETRY_PERIOD * len(path), stop, (args,)).start()
+    threading.Timer(
+        TELEMETRY_PERIOD * len(path), stop, (telemetry_queue, output_queue, controller)
+    ).start()
 
     # Put items into input queue
-    threading.Thread(target=put_queue, args=(args,)).start()
+    threading.Thread(target=put_queue, args=(telemetry_queue, controller, path)).start()
 
     # Read the main queue (worker outputs)
-    threading.Thread(target=read_queue, args=(args, main_logger)).start()
+    threading.Thread(target=read_queue, args=(output_queue, controller, main_logger)).start()
 
     command_worker.command_worker(
         # Place your own arguments here
+        connection,
+        TARGET,
+        output_queue,
+        main_logger,
+        controller,
+        telemetry_queue,
     )
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
diff --git a/tests/integration/test_heartbeat_receiver.py b/tests/integration/test_heartbeat_receiver.py
index 23ff4cc5..62049136 100644
--- a/tests/integration/test_heartbeat_receiver.py
+++ b/tests/integration/test_heartbeat_receiver.py
@@ -48,23 +48,25 @@ def start_drone() -> None:
 # =================================================================================================
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
-def stop(
-    args,  # Add any necessary arguments
-) -> None:
+def stop(controller: worker_controller.WorkerController) -> None:  # Add any necessary arguments
     """
     Stop the workers.
     """
-    pass  # Add logic to stop your worker
+    controller.request_exit()
 
 
 def read_queue(
-    args,  # Add any necessary arguments
+    queue: queue_proxy_wrapper.QueueProxyWrapper,
     main_logger: logger.Logger,
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
     Read and print the output queue.
     """
-    pass  # Add logic to read from your worker's output queue and print it using the logger
+    while not controller.is_exit_requested():
+        if not queue.queue.empty():
+            status = queue.queue.get(timeout=1)
+            main_logger.info(f"Heartbeat status: {status}")
 
 
 # =================================================================================================
@@ -113,24 +115,32 @@ def main() -> int:
     # =============================================================================================
     # Mock starting a worker, since cannot actually start a new process
     # Create a worker controller for your worker
+    controller = worker_controller.WorkerController()
 
     # Create a multiprocess manager for synchronized queues
+    manager = mp.Manager()
 
     # Create your queues
+    queue = queue_proxy_wrapper.QueueProxyWrapper(manager)
 
     # Just set a timer to stop the worker after a while, since the worker infinite loops
     threading.Timer(
         HEARTBEAT_PERIOD * (NUM_TRIALS * 2 + DISCONNECT_THRESHOLD + NUM_DISCONNECTS + 2),
         stop,
-        (args,),
+        args=(controller,),
     ).start()
 
     # Read the main queue (worker outputs)
-    threading.Thread(target=read_queue, args=(args, main_logger)).start()
+    threading.Thread(
+        target=read_queue,
+        args=(
+            queue,
+            main_logger,
+            controller,
+        ),
+    ).start()
 
-    heartbeat_receiver_worker.heartbeat_receiver_worker(
-        # Place your own arguments here
-    )
+    heartbeat_receiver_worker.heartbeat_receiver_worker(connection, main_logger, queue, controller)
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
     # =============================================================================================
diff --git a/tests/integration/test_heartbeat_sender.py b/tests/integration/test_heartbeat_sender.py
index d81402c0..5366696b 100644
--- a/tests/integration/test_heartbeat_sender.py
+++ b/tests/integration/test_heartbeat_sender.py
@@ -44,13 +44,11 @@ def start_drone() -> None:
 # =================================================================================================
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
-def stop(
-    args,  # Add any necessary arguments
-) -> None:
+def stop(controller: worker_controller.WorkerController) -> None:  # Add any necessary arguments
     """
     Stop the workers.
     """
-    pass  # Add logic to stop your worker
+    controller.request_exit()
 
 
 # =================================================================================================
@@ -93,13 +91,12 @@ def main() -> int:
     # =============================================================================================
     # Mock starting a worker, since cannot actually start a new process
     # Create a worker controller for your worker
+    controller = worker_controller.WorkerController()
 
     # Just set a timer to stop the worker after a while, since the worker infinite loops
-    threading.Timer(HEARTBEAT_PERIOD * NUM_TRIALS, stop, (args,)).start()
+    threading.Timer(HEARTBEAT_PERIOD * NUM_TRIALS, stop, (controller,)).start()
 
-    heartbeat_sender_worker.heartbeat_sender_worker(
-        # Place your own arguments here
-    )
+    heartbeat_sender_worker.heartbeat_sender_worker(connection, controller, main_logger)
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
     # =============================================================================================
diff --git a/tests/integration/test_telemetry.py b/tests/integration/test_telemetry.py
index 79f9e497..f8ce34dc 100644
--- a/tests/integration/test_telemetry.py
+++ b/tests/integration/test_telemetry.py
@@ -47,22 +47,29 @@ def start_drone() -> None:
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 def stop(
-    args,  # Add any necessary arguments
+    controller: worker_controller.WorkerController,
+    queue: queue_proxy_wrapper.QueueProxyWrapper,  # Add any necessary arguments
 ) -> None:
     """
     Stop the workers.
     """
-    pass  # Add logic to stop your worker
+    queue.fill_and_drain_queue()
+    controller.request_exit()
 
 
 def read_queue(
-    args,  # Add any necessary arguments
+    queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
     main_logger: logger.Logger,
 ) -> None:
     """
     Read and print the output queue.
     """
-    pass  # Add logic to read from your worker's output queue and print it using the logger
+    # Add logic to read from your worker's output queue and print it using the logger
+    while not controller.is_exit_requested():
+        if not queue.queue.empty():
+            data = queue.queue.get(timeout=1)
+            main_logger.info(f"Telemetry data {data}")
 
 
 # =================================================================================================
@@ -111,19 +118,28 @@ def main() -> int:
     # =============================================================================================
     # Mock starting a worker, since cannot actually start a new process
     # Create a worker controller for your worker
+    controller = worker_controller.WorkerController()
 
     # Create a multiprocess manager for synchronized queues
+    manager = mp.Manager()
 
     # Create your queues
+    queue = queue_proxy_wrapper.QueueProxyWrapper(manager)
 
     # Just set a timer to stop the worker after a while, since the worker infinite loops
-    threading.Timer(TELEMETRY_PERIOD * NUM_TRIALS * 2 + NUM_FAILS, stop, (args,)).start()
+    threading.Timer(
+        TELEMETRY_PERIOD * NUM_TRIALS * 2 + NUM_FAILS, stop, args=(controller, queue)
+    ).start()
 
     # Read the main queue (worker outputs)
-    threading.Thread(target=read_queue, args=(args, main_logger)).start()
+    threading.Thread(target=read_queue, args=(queue, controller, main_logger)).start()
 
     telemetry_worker.telemetry_worker(
         # Put your own arguments here
+        connection,
+        queue,
+        main_logger,
+        controller,
     )
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑