diff --git a/bootcamp_main.py b/bootcamp_main.py
index aa64faa6..cad09bba 100644
--- a/bootcamp_main.py
+++ b/bootcamp_main.py
@@ -30,10 +30,21 @@
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 # Set queue max sizes (<= 0 for infinity)
+HEARTBEAT_RECEIVER_QUEUE_MAX_SIZE = 5
+TELEMETRY_QUEUE_MAX_SIZE = 5
+COMMAND_QUEUE_MAX_SIZE = 5
 
 # 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 position for the drone to face and match altitude
+TARGET = command.Position(10, 20, 30)
+# Run duration in seconds
+RUN_DURATION = 100
 
 # =================================================================================================
 #                            ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -74,43 +85,191 @@ 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 receiver outputs connection status to main
+    heartbeat_receiver_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        HEARTBEAT_RECEIVER_QUEUE_MAX_SIZE,
+    )
+    # Telemetry outputs TelemetryData to Command worker
+    telemetry_to_command_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        TELEMETRY_QUEUE_MAX_SIZE,
+    )
+    # Command outputs command status strings to main
+    command_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, heartbeat_sender_properties = worker_manager.WorkerProperties.create(
+        count=HEARTBEAT_SENDER_WORKER_COUNT,
+        target=heartbeat_sender_worker.heartbeat_sender_worker,
+        work_arguments=(connection,),
+        input_queues=[],
+        output_queues=[],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create arguments for Heartbeat Sender")
+        return -1
+
+    assert heartbeat_sender_properties is not None
 
     # Heartbeat receiver
+    result, heartbeat_receiver_properties = worker_manager.WorkerProperties.create(
+        count=HEARTBEAT_RECEIVER_WORKER_COUNT,
+        target=heartbeat_receiver_worker.heartbeat_receiver_worker,
+        work_arguments=(connection,),
+        input_queues=[],
+        output_queues=[heartbeat_receiver_queue],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create arguments for Heartbeat Receiver")
+        return -1
+
+    assert heartbeat_receiver_properties is not None
 
     # Telemetry
+    result, telemetry_properties = worker_manager.WorkerProperties.create(
+        count=TELEMETRY_WORKER_COUNT,
+        target=telemetry_worker.telemetry_worker,
+        work_arguments=(connection,),
+        input_queues=[],
+        output_queues=[telemetry_to_command_queue],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create arguments for Telemetry")
+        return -1
+
+    assert telemetry_properties is not None
 
     # Command
+    result, command_properties = worker_manager.WorkerProperties.create(
+        count=COMMAND_WORKER_COUNT,
+        target=command_worker.command_worker,
+        work_arguments=(connection, TARGET),
+        input_queues=[telemetry_to_command_queue],
+        output_queues=[command_queue],
+        controller=controller,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create arguments for Command")
+        return -1
+
+    assert command_properties is not None
 
     # Create the workers (processes) and obtain their managers
+    worker_managers: list[worker_manager.WorkerManager] = []
+
+    result, heartbeat_sender_manager = worker_manager.WorkerManager.create(
+        worker_properties=heartbeat_sender_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create manager for Heartbeat Sender")
+        return -1
+
+    assert heartbeat_sender_manager is not None
+    worker_managers.append(heartbeat_sender_manager)
+
+    result, heartbeat_receiver_manager = worker_manager.WorkerManager.create(
+        worker_properties=heartbeat_receiver_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create manager for Heartbeat Receiver")
+        return -1
+
+    assert heartbeat_receiver_manager is not None
+    worker_managers.append(heartbeat_receiver_manager)
+
+    result, telemetry_manager = worker_manager.WorkerManager.create(
+        worker_properties=telemetry_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("Failed to create manager for Telemetry")
+        return -1
+
+    assert telemetry_manager is not None
+    worker_managers.append(telemetry_manager)
+
+    result, command_manager = worker_manager.WorkerManager.create(
+        worker_properties=command_properties,
+        local_logger=main_logger,
+    )
+    if not result:
+        main_logger.error("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()
+    is_connected = True
+
+    while (time.time() - start_time) < RUN_DURATION and is_connected:
+        # Read connection status from heartbeat receiver
+        try:
+            status = heartbeat_receiver_queue.queue.get(timeout=0.1)
+            if status is not None:
+                main_logger.info(f"Connection status: {status}")
+                if status == "disconnected":
+                    is_connected = False
+        except queue.Empty:
+            pass
+
+        # Read command status from command worker
+        try:
+            command_status = command_queue.queue.get(timeout=0.1)
+            if command_status is not None:
+                main_logger.info(f"Command: {command_status}")
+        except queue.Empty:
+            pass
 
     # Stop the processes
+    controller.request_exit()
 
     main_logger.info("Requested exit")
 
     # Fill and drain queues from END TO START
+    command_queue.fill_and_drain_queue()
+    telemetry_to_command_queue.fill_and_drain_queue()
+    heartbeat_receiver_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/logs/2026-01-14_23-24-01/heartbeat_sender_drone_26060.log b/logs/2026-01-14_23-24-01/heartbeat_sender_drone_26060.log
new file mode 100644
index 00000000..e854855d
--- /dev/null
+++ b/logs/2026-01-14_23-24-01/heartbeat_sender_drone_26060.log
@@ -0,0 +1,10 @@
+23:41:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 40] Logger initialized
+23:41:55: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:41:56: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:41:57: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:41:58: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:41:59: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:42:00: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:42:01: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:42:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:42:03: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 56] Drone: Sent incorrect message type or didn't recieve a message in time: None
diff --git a/logs/2026-01-14_23-24-01/main.log b/logs/2026-01-14_23-24-01/main.log
new file mode 100644
index 00000000..43d09bef
--- /dev/null
+++ b/logs/2026-01-14_23-24-01/main.log
@@ -0,0 +1 @@
+23:24:01: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
diff --git a/logs/2026-01-14_23-41-54/heartbeat_sender_drone_26244.log b/logs/2026-01-14_23-41-54/heartbeat_sender_drone_26244.log
new file mode 100644
index 00000000..e364b795
--- /dev/null
+++ b/logs/2026-01-14_23-41-54/heartbeat_sender_drone_26244.log
@@ -0,0 +1,5 @@
+23:43:51: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 40] Logger initialized
+23:44:03: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:44:04: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:44:05: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+23:44:06: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 61] Drone: Most likely sent heartbeats too fast: measured period was 0.9728507995605469s
diff --git a/logs/2026-01-14_23-41-54/heartbeat_sender_worker_31068.log b/logs/2026-01-14_23-41-54/heartbeat_sender_worker_31068.log
new file mode 100644
index 00000000..4d28f7f8
--- /dev/null
+++ b/logs/2026-01-14_23-41-54/heartbeat_sender_worker_31068.log
@@ -0,0 +1,14 @@
+23:41:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 49] Logger initialized
+23:41:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 63] HeartbeatSender created successfully
+23:41:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:41:55: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:41:56: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:41:57: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:41:58: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:41:59: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:42:00: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:42:01: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:42:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:42:03: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:42:04: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:42:05: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 80] Heartbeat sender worker exiting
diff --git a/logs/2026-01-14_23-41-54/main.log b/logs/2026-01-14_23-41-54/main.log
new file mode 100644
index 00000000..a65b2a8c
--- /dev/null
+++ b/logs/2026-01-14_23-41-54/main.log
@@ -0,0 +1,2 @@
+23:41:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+23:41:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_sender.py | main | 91] Connected!
diff --git a/logs/2026-01-14_23-44-02/heartbeat_sender_worker_1000.log b/logs/2026-01-14_23-44-02/heartbeat_sender_worker_1000.log
new file mode 100644
index 00000000..bb7a2500
--- /dev/null
+++ b/logs/2026-01-14_23-44-02/heartbeat_sender_worker_1000.log
@@ -0,0 +1,15 @@
+23:44:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 49] Logger initialized
+23:44:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 63] HeartbeatSender created successfully
+23:44:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:03: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:04: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:05: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:09: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:10: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 73] Heartbeat sent
+23:44:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 80] Heartbeat sender worker exiting
diff --git a/logs/2026-01-14_23-44-02/main.log b/logs/2026-01-14_23-44-02/main.log
new file mode 100644
index 00000000..21f1259d
--- /dev/null
+++ b/logs/2026-01-14_23-44-02/main.log
@@ -0,0 +1,2 @@
+23:44:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+23:44:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_sender.py | main | 91] Connected!
diff --git a/logs/2026-01-18_10-53-02/heartbeat_sender_drone_36704.log b/logs/2026-01-18_10-53-02/heartbeat_sender_drone_36704.log
new file mode 100644
index 00000000..49e2008c
--- /dev/null
+++ b/logs/2026-01-18_10-53-02/heartbeat_sender_drone_36704.log
@@ -0,0 +1,4 @@
+10:53:04: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 40] Logger initialized
+10:53:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:53:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:53:08: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 56] Drone: Sent incorrect message type or didn't recieve a message in time: None
diff --git a/logs/2026-01-18_10-53-02/heartbeat_sender_worker_35588.log b/logs/2026-01-18_10-53-02/heartbeat_sender_worker_35588.log
new file mode 100644
index 00000000..19efe4c2
--- /dev/null
+++ b/logs/2026-01-18_10-53-02/heartbeat_sender_worker_35588.log
@@ -0,0 +1,15 @@
+10:53:05: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 49] Logger initialized
+10:53:05: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 63] HeartbeatSender created successfully
+10:53:05: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:09: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:10: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:53:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 85] Heartbeat sender worker exiting
diff --git a/logs/2026-01-18_10-53-02/main.log b/logs/2026-01-18_10-53-02/main.log
new file mode 100644
index 00000000..e221ecb7
--- /dev/null
+++ b/logs/2026-01-18_10-53-02/main.log
@@ -0,0 +1,2 @@
+10:53:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+10:53:04: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_sender.py | main | 91] Connected!
diff --git a/logs/2026-01-18_10-58-04/heartbeat_sender_drone_5540.log b/logs/2026-01-18_10-58-04/heartbeat_sender_drone_5540.log
new file mode 100644
index 00000000..24826267
--- /dev/null
+++ b/logs/2026-01-18_10-58-04/heartbeat_sender_drone_5540.log
@@ -0,0 +1,12 @@
+10:58:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 40] Logger initialized
+10:58:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:09: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:10: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:17: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 77] Recieved extra heartbeat
diff --git a/logs/2026-01-18_10-58-04/heartbeat_sender_worker_27696.log b/logs/2026-01-18_10-58-04/heartbeat_sender_worker_27696.log
new file mode 100644
index 00000000..f7902035
--- /dev/null
+++ b/logs/2026-01-18_10-58-04/heartbeat_sender_worker_27696.log
@@ -0,0 +1,15 @@
+10:58:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 49] Logger initialized
+10:58:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 63] HeartbeatSender created successfully
+10:58:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:09: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:10: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 85] Heartbeat sender worker exiting
diff --git a/logs/2026-01-18_10-58-04/main.log b/logs/2026-01-18_10-58-04/main.log
new file mode 100644
index 00000000..391d3bcf
--- /dev/null
+++ b/logs/2026-01-18_10-58-04/main.log
@@ -0,0 +1,2 @@
+10:58:04: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+10:58:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_sender.py | main | 91] Connected!
diff --git a/logs/2026-01-18_10-58-49/heartbeat_sender_drone_968.log b/logs/2026-01-18_10-58-49/heartbeat_sender_drone_968.log
new file mode 100644
index 00000000..e165d090
--- /dev/null
+++ b/logs/2026-01-18_10-58-49/heartbeat_sender_drone_968.log
@@ -0,0 +1,12 @@
+10:58:51: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 40] Logger initialized
+10:58:52: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:53: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:55: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:56: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:57: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:58: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:58:59: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:59:00: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:59:01: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+10:59:02: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 77] Recieved extra heartbeat
diff --git a/logs/2026-01-18_10-58-49/heartbeat_sender_worker_38316.log b/logs/2026-01-18_10-58-49/heartbeat_sender_worker_38316.log
new file mode 100644
index 00000000..9432a6d3
--- /dev/null
+++ b/logs/2026-01-18_10-58-49/heartbeat_sender_worker_38316.log
@@ -0,0 +1,15 @@
+10:58:51: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 49] Logger initialized
+10:58:51: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 63] HeartbeatSender created successfully
+10:58:51: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:52: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:53: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:55: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:56: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:57: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:58: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:58:59: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:59:00: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:59:01: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:59:02: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+10:59:03: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 85] Heartbeat sender worker exiting
diff --git a/logs/2026-01-18_10-58-49/main.log b/logs/2026-01-18_10-58-49/main.log
new file mode 100644
index 00000000..4af273c9
--- /dev/null
+++ b/logs/2026-01-18_10-58-49/main.log
@@ -0,0 +1,2 @@
+10:58:49: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+10:58:51: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_sender.py | main | 91] Connected!
diff --git a/logs/command/command_drone_24932.log b/logs/command/command_drone_24932.log
new file mode 100644
index 00000000..079dadb4
--- /dev/null
+++ b/logs/command/command_drone_24932.log
@@ -0,0 +1,28 @@
+10:56:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 45] Logger initialized
+10:56:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 76] Received a valid command
+10:56:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\command_drone.py | main | 83] Passed!
diff --git a/logs/command/command_worker_25252.log b/logs/command/command_worker_25252.log
new file mode 100644
index 00000000..d3d2129d
--- /dev/null
+++ b/logs/command/command_worker_25252.log
@@ -0,0 +1,552 @@
+10:56:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 51] Logger initialized
+10:56:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 65] Command created successfully
+10:56:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 29,
+            x_velocity: 0,
+            y_velocity: 0,
+            z_velocity: 4,
+            roll: None,
+            pitch: None,
+            yaw: 0,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 4.000) m/s
+10:56:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 145] Sent altitude change command: delta_z=1.00m
+10:56:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE ALTITUDE: 1
+10:56:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 31,
+            x_velocity: 0,
+            y_velocity: 0,
+            z_velocity: -2,
+            roll: None,
+            pitch: None,
+            yaw: 0,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 1.000) m/s
+10:56:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 145] Sent altitude change command: delta_z=-1.00m
+10:56:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE ALTITUDE: -1
+10:56:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 30.2,
+            x_velocity: 0,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.1071487177940904,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 0.667) m/s
+10:56:09: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 29.8,
+            x_velocity: 0,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.1071487177940904,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:09: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 0.500) m/s
+10:56:09: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.1071487177940904,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:09: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 0.400) m/s
+10:56:10: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.142055302833977,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:10: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 0.333) m/s
+10:56:10: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.072242132754204,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:10: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 0.286) m/s
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 0,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 2.500, 0.250) m/s
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=63.43deg
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 63.43494882292201
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 10,
+            y: 0,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 0,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 4.444, 0.222) m/s
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=90.00deg
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 90.0
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 20,
+            y: 0,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 0,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 6.000, 0.200) m/s
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=116.57deg
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 116.56505117707799
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 30,
+            y: 0,
+            z: 30,
+            x_velocity: 20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (1.818, 5.455, 0.182) m/s
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=45.00deg
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 45.0
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 30,
+            y: 10,
+            z: 30,
+            x_velocity: 20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (3.333, 5.000, 0.167) m/s
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=63.43deg
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 63.43494882292201
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 30,
+            y: 20,
+            z: 30,
+            x_velocity: 20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (4.615, 4.615, 0.154) m/s
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=90.00deg
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 90.0
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 30,
+            y: 30,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: -20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 3.141592653589793,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (4.286, 2.857, 0.143) m/s
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=26.57deg
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 26.565051177077994
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 20,
+            y: 30,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: -20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -3.141592653589793,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (4.000, 1.333, 0.133) m/s
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=45.00deg
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 45.0
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 10,
+            y: 30,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: -20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 3.141592653589793,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (3.750, 0.000, 0.125) m/s
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=90.00deg
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 90.0
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 30,
+            z: 30,
+            x_velocity: -20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (2.353, 0.000, 0.118) m/s
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=45.00deg
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 45.0
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 20,
+            z: 30,
+            x_velocity: -20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (1.111, 0.000, 0.111) m/s
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=90.00deg
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 90.0
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 10,
+            z: 30,
+            x_velocity: -20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 0.105) m/s
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=135.00deg
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: 135.0
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 0,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 1.000, 0.100) m/s
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-26.57deg
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -26.565051177077994
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 10,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 1.905, 0.095) m/s
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-45.00deg
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -45.0
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 20,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: 20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 2.727, 0.091) m/s
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-90.00deg
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -90.0
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 0,
+            y: 30,
+            z: 30,
+            x_velocity: 20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 0,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.870, 2.609, 0.087) m/s
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-45.00deg
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -45.0
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 10,
+            y: 30,
+            z: 30,
+            x_velocity: 20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 0,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (1.667, 2.500, 0.083) m/s
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-90.00deg
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -90.0
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 20,
+            y: 30,
+            z: 30,
+            x_velocity: 20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 0,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (2.400, 2.400, 0.080) m/s
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-135.00deg
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -135.0
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 30,
+            y: 30,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: -20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (2.308, 1.538, 0.077) m/s
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-63.43deg
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -63.43494882292201
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 30,
+            y: 20,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: -20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (2.222, 0.741, 0.074) m/s
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-90.00deg
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -90.0
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 30,
+            y: 10,
+            z: 30,
+            x_velocity: 0,
+            y_velocity: -20,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -1.5707963267948966,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (2.143, 0.000, 0.071) m/s
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-116.57deg
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -116.56505117707799
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 30,
+            y: 0,
+            z: 30,
+            x_velocity: -20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -3.141592653589793,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (1.379, 0.000, 0.069) m/s
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-45.00deg
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -45.0
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 20,
+            y: 0,
+            z: 30,
+            x_velocity: -20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: 3.141592653589793,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.667, 0.000, 0.067) m/s
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-63.43deg
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -63.43494882292201
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 81] Received telemetry data: {
+            time_since_boot: None,
+            x: 10,
+            y: 0,
+            z: 30,
+            x_velocity: -20,
+            y_velocity: 0,
+            z_velocity: 0,
+            roll: None,
+            pitch: None,
+            yaw: -3.141592653589793,
+            roll_speed: None,
+            pitch_speed: None,
+            yaw_speed: None
+        }
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 125] Average velocity: (0.000, 0.000, 0.065) m/s
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command.py | run | 191] Sent yaw change command: delta_yaw=-90.00deg
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 89] Command status sent to queue: CHANGE YAW: -90.0
+10:56:24: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 78] Received sentinel value, exiting
+10:56:24: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\command\command_worker.py | command_worker | 95] Command worker exiting
diff --git a/logs/command/main.log b/logs/command/main.log
new file mode 100644
index 00000000..175c7845
--- /dev/null
+++ b/logs/command/main.log
@@ -0,0 +1,28 @@
+10:56:03: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+10:56:06: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | main | 158] Connected!
+10:56:07: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE ALTITUDE: 1
+10:56:08: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE ALTITUDE: -1
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 63.43494882292201
+10:56:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 90.0
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 116.56505117707799
+10:56:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 45.0
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 63.43494882292201
+10:56:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 90.0
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 26.565051177077994
+10:56:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 45.0
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 90.0
+10:56:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 45.0
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 90.0
+10:56:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: 135.0
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -26.565051177077994
+10:56:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -45.0
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -90.0
+10:56:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -45.0
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -90.0
+10:56:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -135.0
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -63.43494882292201
+10:56:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -90.0
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -116.56505117707799
+10:56:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -45.0
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -63.43494882292201
+10:56:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_command.py | read_queue | 94] Worker output: CHANGE YAW: -90.0
diff --git a/logs/heartbeat_receiver/heartbeat_receiver_drone_37332.log b/logs/heartbeat_receiver/heartbeat_receiver_drone_37332.log
new file mode 100644
index 00000000..ba3bb751
--- /dev/null
+++ b/logs/heartbeat_receiver/heartbeat_receiver_drone_37332.log
@@ -0,0 +1,13 @@
+10:54:33: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | main | 42] Logger initialized
+10:54:33: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:35: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:36: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:37: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:46: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:47: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:48: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:49: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:50: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:52: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | send_heartbeats | 59] Drone: Sent a heartbeat
+10:54:53: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_receiver_drone.py | main | 79] Passesd!
diff --git a/logs/heartbeat_receiver/heartbeat_receiver_worker_16320.log b/logs/heartbeat_receiver/heartbeat_receiver_worker_16320.log
new file mode 100644
index 00000000..b5f21b8b
--- /dev/null
+++ b/logs/heartbeat_receiver/heartbeat_receiver_worker_16320.log
@@ -0,0 +1,55 @@
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 47] Logger initialized
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 61] HeartbeatReceiver created successfully
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:35: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (1/5)
+10:54:35: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:35: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:35: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:36: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (1/5)
+10:54:36: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:36: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:36: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:37: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:37: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:38: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (1/5)
+10:54:38: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:39: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (2/5)
+10:54:39: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:40: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (3/5)
+10:54:40: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:41: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (4/5)
+10:54:41: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:42: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (5/5)
+10:54:42: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 98] Connection lost: too many missed heartbeats
+10:54:42: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: disconnected
+10:54:43: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (6/5)
+10:54:43: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 98] Connection lost: too many missed heartbeats
+10:54:43: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: disconnected
+10:54:44: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (7/5)
+10:54:44: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 98] Connection lost: too many missed heartbeats
+10:54:44: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: disconnected
+10:54:45: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (8/5)
+10:54:45: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 98] Connection lost: too many missed heartbeats
+10:54:45: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: disconnected
+10:54:46: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:46: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:47: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:47: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:48: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:48: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:49: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:49: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:50: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:50: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:51: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (1/5)
+10:54:51: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:52: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 87] Received heartbeat from drone
+10:54:52: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:53: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (1/5)
+10:54:53: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:54: [WARNING] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver.py | run | 91] Missed heartbeat (2/5)
+10:54:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 74] Connection status: connected
+10:54:54: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_receiver_worker.py | heartbeat_receiver_worker | 76] Heartbeat receiver worker exiting
diff --git a/logs/heartbeat_receiver/main.log b/logs/heartbeat_receiver/main.log
new file mode 100644
index 00000000..f0def0a6
--- /dev/null
+++ b/logs/heartbeat_receiver/main.log
@@ -0,0 +1,25 @@
+10:54:31: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+10:54:33: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | main | 130] Connected!
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:34: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:35: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:35: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:36: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:36: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:37: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:38: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:39: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:40: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:41: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:42: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: disconnected
+10:54:43: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: disconnected
+10:54:44: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: disconnected
+10:54:45: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: disconnected
+10:54:46: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:47: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:48: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:49: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:50: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:51: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:52: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
+10:54:53: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_receiver.py | read_queue | 86] Worker output: connected
diff --git a/logs/heartbeat_sender/heartbeat_sender_drone_37752.log b/logs/heartbeat_sender/heartbeat_sender_drone_37752.log
new file mode 100644
index 00000000..e75de787
--- /dev/null
+++ b/logs/heartbeat_sender/heartbeat_sender_drone_37752.log
@@ -0,0 +1,12 @@
+11:00:12: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 40] Logger initialized
+11:00:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:23: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 71] Drone: Recieved heartbeat!
+11:00:24: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\heartbeat_sender_drone.py | main | 80] Passed!
diff --git a/logs/heartbeat_sender/heartbeat_sender_worker_34856.log b/logs/heartbeat_sender/heartbeat_sender_worker_34856.log
new file mode 100644
index 00000000..78213445
--- /dev/null
+++ b/logs/heartbeat_sender/heartbeat_sender_worker_34856.log
@@ -0,0 +1,14 @@
+11:00:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 49] Logger initialized
+11:00:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 63] HeartbeatSender created successfully
+11:00:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:14: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:15: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:23: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 75] Heartbeat sent
+11:00:24: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\heartbeat\heartbeat_sender_worker.py | heartbeat_sender_worker | 85] Heartbeat sender worker exiting
diff --git a/logs/heartbeat_sender/main.log b/logs/heartbeat_sender/main.log
new file mode 100644
index 00000000..70580fc1
--- /dev/null
+++ b/logs/heartbeat_sender/main.log
@@ -0,0 +1,2 @@
+11:00:11: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+11:00:13: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_heartbeat_sender.py | main | 91] Connected!
diff --git a/logs/telemetry/main.log b/logs/telemetry/main.log
new file mode 100644
index 00000000..67c0184f
--- /dev/null
+++ b/logs/telemetry/main.log
@@ -0,0 +1,302 @@
+10:55:16: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\common\modules\logger\logger_main_setup.py | setup_main_logger | 62] main logger initialized
+10:55:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | main | 128] Connected!
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:25: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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
+        }
+10:55:30: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\test_telemetry.py | read_queue | 84] Worker output: {
+            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_37292.log b/logs/telemetry/telemetry_drone_37292.log
new file mode 100644
index 00000000..48e2e390
--- /dev/null
+++ b/logs/telemetry/telemetry_drone_37292.log
@@ -0,0 +1,52 @@
+10:55:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | main | 45] Logger initialized
+10:55:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 0
+10:55:17: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 0
+10:55:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 1
+10:55:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 1
+10:55:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 2
+10:55:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 3
+10:55:18: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 2
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 4
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 3
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 5
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 6
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 4
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 7
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 5
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 8
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 9
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 6
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 10
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 7
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 11
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 12
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 8
+10:55:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 13
+10:55:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 9
+10:55:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 14
+10:55:25: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 0
+10:55:25: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 0
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 1
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 1
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 2
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 2
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 3
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 4
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 3
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 5
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 4
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 6
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 7
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 5
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 8
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 6
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 9
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 10
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 7
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 11
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 8
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 12
+10:55:30: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 13
+10:55:30: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 75] Drone: Sent attitude 9
+10:55:30: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | send_telemetry | 95] Drone: Sent position 14
+10:55:30: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\tests\integration\mock_drones\telemetry_drone.py | main | 134] Passed!
diff --git a/logs/telemetry/telemetry_worker_28736.log b/logs/telemetry/telemetry_worker_28736.log
new file mode 100644
index 00000000..4954bd4f
--- /dev/null
+++ b/logs/telemetry/telemetry_worker_28736.log
@@ -0,0 +1,661 @@
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 47] Logger initialized
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 61] Telemetry created successfully
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=0.0
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=0.0, y=0.0, z=0.0
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=1.0471975803375244
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=0.5, y=0.0, z=0.0
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=2.094395160675049
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=3.1415927410125732
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=1.0, y=0.0, z=0.0
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=-2.094395160675049
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=1.5, y=0.0, z=0.0
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=-1.0471975803375244
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=0.0
+10:55:19: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=2.0, y=0.0, z=0.0
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:19: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:20: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=1.0471975803375244
+10:55:20: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=2.5, y=0.0, z=0.0
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:20: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=2.094395160675049
+10:55:20: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=3.1415927410125732
+10:55:20: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=3.0, y=0.0, z=0.0
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:20: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:21: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=-2.094395160675049
+10:55:21: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=3.5, y=0.0, z=0.0
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:21: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=-1.0471975803375244
+10:55:21: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=0.0
+10:55:21: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=4.0, y=0.0, z=0.0
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:21: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:22: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=1.0471975803375244
+10:55:22: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=4.5, y=0.0, z=0.0
+10:55:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:22: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:22: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=2.094395160675049
+10:55:23: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 187] Telemetry timeout: did not receive both ATTITUDE and LOCAL_POSITION_NED
+10:55:23: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=3.0
+10:55:24: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 187] Telemetry timeout: did not receive both ATTITUDE and LOCAL_POSITION_NED
+10:55:24: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=3.0, y=2.0, z=1.0
+10:55:25: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 187] Telemetry timeout: did not receive both ATTITUDE and LOCAL_POSITION_NED
+10:55:25: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=0.0
+10:55:25: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=0.0, y=0.0, z=0.0
+10:55:25: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:25: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:26: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=0.3333333432674408, y=0.0, z=0.0
+10:55:26: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=1.5707963705062866
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:26: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=0.6666666865348816, y=0.0, z=0.0
+10:55:26: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=3.1415927410125732
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:26: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:26: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=1.0, y=0.0, z=0.0
+10:55:27: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=1.3333333730697632, y=0.0, z=0.0
+10:55:27: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=-1.5707963705062866
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:27: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=1.6666666269302368, y=0.0, z=0.0
+10:55:27: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=0.0
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:27: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:27: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=2.0, y=0.0, z=0.0
+10:55:28: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=2.3333332538604736, y=0.0, z=0.0
+10:55:28: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=1.5707963705062866
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:28: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=2.6666667461395264, y=0.0, z=0.0
+10:55:28: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=3.1415927410125732
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:28: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:28: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=3.0, y=0.0, z=0.0
+10:55:29: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=3.3333332538604736, y=0.0, z=0.0
+10:55:29: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=-1.5707963705062866
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:29: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=3.6666667461395264, y=0.0, z=0.0
+10:55:29: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=0.0
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:29: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:29: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=4.0, y=0.0, z=0.0
+10:55:30: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=4.333333492279053, y=0.0, z=0.0
+10:55:30: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 148] Received ATTITUDE: yaw=1.5707963705062866
+10:55:30: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 179] 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
+        }
+10:55:30: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 74] Telemetry data sent to queue: {
+            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
+        }
+10:55:30: [DEBUG] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 153] Received LOCAL_POSITION_NED: x=4.666666507720947, y=0.0, z=0.0
+10:55:31: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 187] Telemetry timeout: did not receive both ATTITUDE and LOCAL_POSITION_NED
+10:55:32: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 187] Telemetry timeout: did not receive both ATTITUDE and LOCAL_POSITION_NED
+10:55:33: [ERROR] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry.py | run | 187] Telemetry timeout: did not receive both ATTITUDE and LOCAL_POSITION_NED
+10:55:33: [INFO] [C:\Users\Mark\Desktop\WARG\autonomy-bootcamp-2026-p2\modules\telemetry\telemetry_worker.py | telemetry_worker | 76] Telemetry worker exiting
diff --git a/modules/command/command.py b/modules/command/command.py
index d85a459a..97eb3e2d 100644
--- a/modules/command/command.py
+++ b/modules/command/command.py
@@ -24,6 +24,16 @@ def __init__(self, x: float, y: float, z: float) -> None:
 # =================================================================================================
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
+# Tolerance for altitude adjustment (meters)
+HEIGHT_TOLERANCE = 0.5
+# Tolerance for yaw adjustment (degrees)
+ANGLE_TOLERANCE = 5.0
+# Speed for altitude change (m/s)
+Z_SPEED = 1.0
+# Turning speed for yaw change (deg/s)
+TURNING_SPEED = 5.0
+
+
 class Command:  # pylint: disable=too-many-instance-attributes
     """
     Command class to make a decision based on recieved telemetry,
@@ -37,44 +47,152 @@ def create(
         cls,
         connection: mavutil.mavfile,
         target: Position,
-        args,  # Put your own arguments here
         local_logger: logger.Logger,
-    ):
+    ) -> "tuple[True, Command] | tuple[False, None]":
         """
         Falliable create (instantiation) method to create a Command object.
+
+        Parameters:
+            connection: MAVLink connection to the drone.
+            target: Target position to face and match altitude.
+            local_logger: Logger instance for logging messages.
+
+        Returns:
+            A tuple containing success status and the Command object (or None on failure).
         """
-        pass  #  Create a Command object
+        if connection is None:
+            return False, None
+
+        if target is None:
+            return False, None
+
+        if local_logger is None:
+            return False, None
+
+        return True, Command(cls.__private_key, connection, target, local_logger)
 
     def __init__(
         self,
         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.__connection = connection
+        self.__target = target
+        self.__logger = local_logger
+
+        # For calculating average velocity
+        self.__total_velocity_x = 0.0
+        self.__total_velocity_y = 0.0
+        self.__total_velocity_z = 0.0
+        self.__sample_count = 0
 
     def run(
         self,
-        args,  # Put your own arguments here
-    ):
+        telemetry_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
 
-        # Use COMMAND_LONG (76) message, assume the target_system=1 and target_componenet=0
-        # The appropriate commands to use are instructed below
+        Parameters:
+            telemetry_data: Current telemetry data from the drone.
 
-        # 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}"
-
-        # 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
+        Returns:
+            A tuple (success, status_string) describing the command sent.
+        """
+        if telemetry_data is None:
+            self.__logger.error("Received None telemetry data")
+            return False, None
+
+        # Update average velocity calculation
+        if (
+            telemetry_data.x_velocity is not None
+            and telemetry_data.y_velocity is not None
+            and telemetry_data.z_velocity is not None
+        ):
+            self.__total_velocity_x += telemetry_data.x_velocity
+            self.__total_velocity_y += telemetry_data.y_velocity
+            self.__total_velocity_z += telemetry_data.z_velocity
+            self.__sample_count += 1
+
+            # Calculate and log average velocity
+            avg_vx = self.__total_velocity_x / self.__sample_count
+            avg_vy = self.__total_velocity_y / self.__sample_count
+            avg_vz = self.__total_velocity_z / self.__sample_count
+            self.__logger.info(f"Average velocity: ({avg_vx:.3f}, {avg_vy:.3f}, {avg_vz:.3f}) m/s")
+
+        # Check if altitude adjustment is needed
+        if telemetry_data.z is not None:
+            delta_z = self.__target.z - telemetry_data.z
+            if abs(delta_z) > HEIGHT_TOLERANCE:
+                # Send altitude change command
+                self.__connection.mav.command_long_send(
+                    1,  # target_system
+                    0,  # target_component
+                    mavutil.mavlink.MAV_CMD_CONDITION_CHANGE_ALT,  # command (113)
+                    0,  # confirmation
+                    Z_SPEED,  # param1: descent/ascend rate (m/s)
+                    0,  # param2: empty
+                    0,  # param3: empty
+                    0,  # param4: empty
+                    0,  # param5: empty
+                    0,  # param6: empty
+                    self.__target.z,  # param7: target altitude
+                )
+                self.__logger.info(f"Sent altitude change command: delta_z={delta_z:.2f}m")
+                return True, f"CHANGE ALTITUDE: {delta_z}"
+
+        # Check if yaw adjustment is needed
+        if (
+            telemetry_data.x is not None
+            and telemetry_data.y is not None
+            and telemetry_data.yaw is not None
+        ):
+            # Calculate the angle to the target from the drone's position
+            dx = self.__target.x - telemetry_data.x
+            dy = self.__target.y - telemetry_data.y
+
+            # Calculate desired yaw (angle to target)
+            desired_yaw = math.atan2(dy, dx)
+
+            # Calculate the difference between current yaw and desired yaw
+            delta_yaw = desired_yaw - telemetry_data.yaw
+
+            # Normalize to [-pi, pi]
+            while delta_yaw > math.pi:
+                delta_yaw -= 2 * math.pi
+            while delta_yaw < -math.pi:
+                delta_yaw += 2 * math.pi
+
+            # Convert to degrees for comparison
+            delta_yaw_deg = math.degrees(delta_yaw)
+
+            if abs(delta_yaw_deg) > ANGLE_TOLERANCE:
+                # Determine direction: 1 = CCW (positive), -1 = CW (negative)
+                direction = 1.0 if delta_yaw_deg >= 0 else -1.0
+
+                # Send yaw change command
+                self.__connection.mav.command_long_send(
+                    1,  # target_system
+                    0,  # target_component
+                    mavutil.mavlink.MAV_CMD_CONDITION_YAW,  # command (115)
+                    0,  # confirmation
+                    abs(delta_yaw_deg),  # param1: target angle (degrees)
+                    TURNING_SPEED,  # param2: angular speed (deg/s)
+                    direction,  # param3: direction (1=CCW, -1=CW)
+                    1.0,  # param4: relative (1=relative to current)
+                    0,  # param5: empty
+                    0,  # param6: empty
+                    0,  # param7: empty
+                )
+                self.__logger.info(f"Sent yaw change command: delta_yaw={delta_yaw_deg:.2f}deg")
+                return True, f"CHANGE YAW: {delta_yaw_deg}"
+
+        # No command needed
+        return False, None
 
 
 # =================================================================================================
diff --git a/modules/command/command_worker.py b/modules/command/command_worker.py
index 8a3b0ff3..7bff5e37 100644
--- a/modules/command/command_worker.py
+++ b/modules/command/command_worker.py
@@ -19,13 +19,19 @@
 def command_worker(
     connection: mavutil.mavfile,
     target: command.Position,
-    args,  # Place your own arguments here
-    # Add other necessary worker arguments here
+    input_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
-    Worker process.
+    Worker process that makes decisions based on telemetry data and sends commands.
 
-    args... describe what the arguments are
+    Parameters:
+        connection: MAVLink connection to the drone.
+        target: Target position to face and match altitude.
+        input_queue: Queue to receive TelemetryData from.
+        output_queue: Queue to output command status strings.
+        controller: WorkerController for managing pause/exit requests.
     """
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -48,8 +54,45 @@ def command_worker(
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
     # Instantiate class object (command.Command)
+    result, command_instance = command.Command.create(connection, target, local_logger)
+    if not result:
+        local_logger.error("Failed to create Command")
+        return
+
+    # Get Pylance to stop complaining
+    assert command_instance is not None
+
+    local_logger.info("Command created successfully")
 
     # Main loop: do work.
+    while not controller.is_exit_requested():
+        # Check if pause has been requested
+        controller.check_pause()
+
+        try:
+            # Get telemetry data from input queue with timeout
+            telemetry_data = input_queue.queue.get(timeout=0.5)
+
+            # Check for sentinel value
+            if telemetry_data is None:
+                local_logger.info("Received sentinel value, exiting")
+                break
+
+            local_logger.info(f"Received telemetry data: {telemetry_data}")
+
+            # Process the telemetry data and send commands
+            result, status = command_instance.run(telemetry_data)
+
+            if result and status is not None:
+                # Output the command status to the queue
+                output_queue.queue.put(status)
+                local_logger.info(f"Command status sent to queue: {status}")
+
+        except Exception:  # pylint: disable=broad-except
+            # Queue.get with timeout raises Empty exception if no data
+            continue
+
+    local_logger.info("Command worker exiting")
 
 
 # =================================================================================================
diff --git a/modules/common b/modules/common
index c0ffaa5c..1f7505ef 160000
--- a/modules/common
+++ b/modules/common
@@ -1 +1 @@
-Subproject commit c0ffaa5cb4bc55a1eeb0f02d4418eaab10c69b81
+Subproject commit 1f7505eff1a4d03b4e919fc79bbdbd944fadde68
diff --git a/modules/heartbeat/heartbeat_receiver.py b/modules/heartbeat/heartbeat_receiver.py
index 3c107f77..4c05fa39 100644
--- a/modules/heartbeat/heartbeat_receiver.py
+++ b/modules/heartbeat/heartbeat_receiver.py
@@ -10,9 +10,18 @@
 # =================================================================================================
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
+# Heartbeat period in seconds
+HEARTBEAT_PERIOD = 1.0
+# Number of missed heartbeats before considered disconnected
+DISCONNECT_THRESHOLD = 5
+
+
 class HeartbeatReceiver:
     """
-    HeartbeatReceiver class to send a heartbeat
+    HeartbeatReceiver class to receive heartbeats from the drone and track connection status.
+
+    Receives HEARTBEAT messages at 1Hz. If connected, becomes disconnected after
+    missing DISCONNECT_THRESHOLD consecutive heartbeats.
     """
 
     __private_key = object()
@@ -21,34 +30,83 @@ class HeartbeatReceiver:
     def create(
         cls,
         connection: mavutil.mavfile,
-        args,  # Put your own arguments here
         local_logger: logger.Logger,
-    ):
+    ) -> "tuple[True, HeartbeatReceiver] | tuple[False, None]":
         """
         Falliable create (instantiation) method to create a HeartbeatReceiver object.
+
+        Parameters:
+            connection: MAVLink connection to the drone.
+            local_logger: Logger instance for logging messages.
+
+        Returns:
+            A tuple containing success status and the HeartbeatReceiver object (or None on failure).
         """
-        pass  # Create a HeartbeatReceiver object
+        if connection is None:
+            return False, None
+
+        if local_logger is None:
+            return False, None
+
+        return True, HeartbeatReceiver(cls.__private_key, connection, local_logger)
 
     def __init__(
         self,
         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.__connection = connection
+        self.__logger = local_logger
+        self.__missed_heartbeats = 0
+        self.__is_connected = False
 
-    def run(
-        self,
-        args,  # Put your own arguments here
-    ):
+    def run(self) -> tuple[bool, str]:
         """
-        Attempt to recieve a heartbeat message.
+        Attempt to receive a heartbeat message.
         If disconnected for over a threshold number of periods,
         the connection is considered disconnected.
+
+        Returns:
+            A tuple (success, status_string) where status_string is "connected" or "disconnected".
         """
-        pass
+        try:
+            # Try to receive a heartbeat message with timeout
+            msg = self.__connection.recv_match(
+                type="HEARTBEAT",
+                blocking=True,
+                timeout=HEARTBEAT_PERIOD,
+            )
+
+            if msg is not None and msg.get_type() == "HEARTBEAT":
+                # Successfully received a heartbeat
+                self.__missed_heartbeats = 0
+                self.__is_connected = True
+                self.__logger.info("Received heartbeat from drone")
+            else:
+                # Did not receive a heartbeat in time
+                self.__missed_heartbeats += 1
+                self.__logger.warning(
+                    f"Missed heartbeat ({self.__missed_heartbeats}/{DISCONNECT_THRESHOLD})"
+                )
+
+                # Check if we have exceeded the disconnect threshold
+                if self.__missed_heartbeats >= DISCONNECT_THRESHOLD:
+                    self.__is_connected = False
+                    self.__logger.warning("Connection lost: too many missed heartbeats")
+
+        except Exception as e:  # pylint: disable=broad-except
+            self.__missed_heartbeats += 1
+            self.__logger.error(f"Error receiving heartbeat: {e}")
+
+            if self.__missed_heartbeats >= DISCONNECT_THRESHOLD:
+                self.__is_connected = False
+
+        # Return current connection status
+        status = "connected" if self.__is_connected else "disconnected"
+        return True, status
 
 
 # =================================================================================================
diff --git a/modules/heartbeat/heartbeat_receiver_worker.py b/modules/heartbeat/heartbeat_receiver_worker.py
index 0e90e3d9..4aaef60d 100644
--- a/modules/heartbeat/heartbeat_receiver_worker.py
+++ b/modules/heartbeat/heartbeat_receiver_worker.py
@@ -18,13 +18,16 @@
 # =================================================================================================
 def heartbeat_receiver_worker(
     connection: mavutil.mavfile,
-    args,  # Place your own arguments here
-    # Add other necessary worker arguments here
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
-    Worker process.
+    Worker process that receives heartbeat messages from the drone and tracks connection status.
 
-    args... describe what the arguments are
+    Parameters:
+        connection: MAVLink connection to the drone.
+        output_queue: Queue to output connection status strings ("connected" or "disconnected").
+        controller: WorkerController for managing pause/exit requests.
     """
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -47,8 +50,30 @@ def heartbeat_receiver_worker(
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
     # Instantiate class object (heartbeat_receiver.HeartbeatReceiver)
+    result, receiver = heartbeat_receiver.HeartbeatReceiver.create(connection, local_logger)
+    if not result:
+        local_logger.error("Failed to create HeartbeatReceiver")
+        return
+
+    # Get Pylance to stop complaining
+    assert receiver is not None
+
+    local_logger.info("HeartbeatReceiver created successfully")
 
     # Main loop: do work.
+    while not controller.is_exit_requested():
+        # Check if pause has been requested
+        controller.check_pause()
+
+        # Receive heartbeat and check connection status
+        result, status = receiver.run()
+
+        if result:
+            # Output the connection status to the queue
+            output_queue.queue.put(status)
+            local_logger.info(f"Connection status: {status}")
+
+    local_logger.info("Heartbeat receiver worker exiting")
 
 
 # =================================================================================================
diff --git a/modules/heartbeat/heartbeat_sender.py b/modules/heartbeat/heartbeat_sender.py
index 627cdde1..fe0d0666 100644
--- a/modules/heartbeat/heartbeat_sender.py
+++ b/modules/heartbeat/heartbeat_sender.py
@@ -10,7 +10,8 @@
 # =================================================================================================
 class HeartbeatSender:
     """
-    HeartbeatSender class to send a heartbeat
+    HeartbeatSender class to send a heartbeat message to the drone.
+    Sends HEARTBEAT messages at a regular interval (once per second).
     """
 
     __private_key = object()
@@ -19,31 +20,52 @@ class HeartbeatSender:
     def create(
         cls,
         connection: mavutil.mavfile,
-        args,  # Put your own arguments here
     ) -> "tuple[True, HeartbeatSender] | tuple[False, None]":
         """
         Falliable create (instantiation) method to create a HeartbeatSender object.
+
+        Parameters:
+            connection: MAVLink connection to the drone.
+
+        Returns:
+            A tuple containing success status and the HeartbeatSender object (or None on failure).
         """
-        pass  # Create a HeartbeatSender object
+        if connection is None:
+            return False, None
+
+        return True, HeartbeatSender(cls.__private_key, connection)
 
     def __init__(
         self,
         key: object,
         connection: mavutil.mavfile,
-        args,  # Put your own arguments here
-    ):
+    ) -> None:
         assert key is HeartbeatSender.__private_key, "Use create() method"
 
-        # Do any intializiation here
+        self.__connection = connection
 
-    def run(
-        self,
-        args,  # Put your own arguments here
-    ):
+    def run(self) -> tuple[bool, None]:
         """
         Attempt to send a heartbeat message.
+
+        Sends a MAVLink HEARTBEAT message with:
+        - type: MAV_TYPE_GCS (Ground Control Station)
+        - autopilot: MAV_AUTOPILOT_INVALID (not an autopilot)
+
+        Returns:
+            A tuple (success, None) where success indicates if the heartbeat was sent.
         """
-        pass  # Send a heartbeat message
+        try:
+            self.__connection.mav.heartbeat_send(
+                mavutil.mavlink.MAV_TYPE_GCS,  # Ground Control Station
+                mavutil.mavlink.MAV_AUTOPILOT_INVALID,  # Not an autopilot
+                0,  # base_mode
+                0,  # custom_mode
+                0,  # system_status
+            )
+            return True, None
+        except Exception:  # pylint: disable=broad-except
+            return False, None
 
 
 # =================================================================================================
diff --git a/modules/heartbeat/heartbeat_sender_worker.py b/modules/heartbeat/heartbeat_sender_worker.py
index ba959083..46fdcfdd 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 in seconds
+HEARTBEAT_PERIOD = 1
+
+
 def heartbeat_sender_worker(
     connection: mavutil.mavfile,
-    args,  # Place your own arguments here
-    # Add other necessary worker arguments here
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
-    Worker process.
+    Worker process that sends heartbeat messages to the drone at 1Hz.
 
-    args... describe what the arguments are
+    Parameters:
+        connection: MAVLink connection to the drone.
+        controller: WorkerController for managing pause/exit requests.
     """
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -47,8 +52,37 @@ def heartbeat_sender_worker(
     #                          ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
     # =============================================================================================
     # Instantiate class object (heartbeat_sender.HeartbeatSender)
+    result, sender = heartbeat_sender.HeartbeatSender.create(connection)
+    if not result:
+        local_logger.error("Failed to create HeartbeatSender")
+        return
+
+    # Get Pylance to stop complaining
+    assert sender is not None
+
+    local_logger.info("HeartbeatSender created successfully")
 
     # Main loop: do work.
+    next_heartbeat_time = time.time()
+
+    while not controller.is_exit_requested():
+        # Check if pause has been requested
+        controller.check_pause()
+
+        # Send heartbeat
+        result, _ = sender.run()
+        if result:
+            local_logger.info("Heartbeat sent")
+        else:
+            local_logger.error("Failed to send heartbeat")
+
+        # Schedule next heartbeat exactly 1 second from the last scheduled time
+        next_heartbeat_time += HEARTBEAT_PERIOD
+        sleep_time = next_heartbeat_time - time.time()
+        if sleep_time > 0:
+            time.sleep(sleep_time)
+
+    local_logger.info("Heartbeat sender worker exiting")
 
 
 # =================================================================================================
diff --git a/modules/telemetry/telemetry.py b/modules/telemetry/telemetry.py
index 0174eeb7..c226a2e8 100644
--- a/modules/telemetry/telemetry.py
+++ b/modules/telemetry/telemetry.py
@@ -65,9 +65,16 @@ def __str__(self) -> str:
 # =================================================================================================
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
+# Timeout in seconds for receiving both messages
+TELEMETRY_TIMEOUT = 1.0
+
+
 class Telemetry:
     """
     Telemetry class to read position and attitude (orientation).
+
+    Receives ATTITUDE (30) and LOCAL_POSITION_NED (32) messages and combines them
+    into a single TelemetryData object.
     """
 
     __private_key = object()
@@ -76,37 +83,113 @@ class Telemetry:
     def create(
         cls,
         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.
+
+        Parameters:
+            connection: MAVLink connection to the drone.
+            local_logger: Logger instance for logging messages.
+
+        Returns:
+            A tuple containing success status and the Telemetry object (or None on failure).
         """
-        pass  # Create a Telemetry object
+        if connection is None:
+            return False, None
+
+        if local_logger is None:
+            return False, None
+
+        return True, Telemetry(cls.__private_key, connection, local_logger)
 
     def __init__(
         self,
         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.__logger = local_logger
 
-    def run(
-        self,
-        args,  # Put your own arguments here
-    ):
+    def run(self) -> "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.
+
+        Returns:
+            A tuple (success, TelemetryData) or (False, None) on timeout.
         """
-        # 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()
+        remaining_timeout = TELEMETRY_TIMEOUT
+
+        attitude_data = None
+        position_data = None
+        attitude_time = 0
+        position_time = 0
+
+        # Keep trying to receive both messages within the timeout
+        while remaining_timeout > 0:
+            # Try to receive any telemetry message
+            msg = self.__connection.recv_match(
+                type=["ATTITUDE", "LOCAL_POSITION_NED"],
+                blocking=True,
+                timeout=remaining_timeout,
+            )
+
+            if msg is not None:
+                msg_type = msg.get_type()
+
+                if msg_type == "ATTITUDE":
+                    attitude_data = msg
+                    attitude_time = msg.time_boot_ms
+                    self.__logger.debug(f"Received ATTITUDE: yaw={msg.yaw}")
+
+                elif msg_type == "LOCAL_POSITION_NED":
+                    position_data = msg
+                    position_time = msg.time_boot_ms
+                    self.__logger.debug(
+                        f"Received LOCAL_POSITION_NED: x={msg.x}, y={msg.y}, z={msg.z}"
+                    )
+
+            # Check if we have both messages
+            if attitude_data is not None and position_data is not None:
+                # Use the most recent timestamp
+                most_recent_time = max(attitude_time, position_time)
+
+                # Create TelemetryData with combined data
+                # Note: LOCAL_POSITION_NED uses NED coordinates, but we're treating it as x-y-z
+                # as per bootcamp instructions (x=right, y=forward, z=up)
+                telemetry_data = TelemetryData(
+                    time_since_boot=most_recent_time,
+                    x=position_data.x,
+                    y=position_data.y,
+                    z=position_data.z,
+                    x_velocity=position_data.vx,
+                    y_velocity=position_data.vy,
+                    z_velocity=position_data.vz,
+                    roll=attitude_data.roll,
+                    pitch=attitude_data.pitch,
+                    yaw=attitude_data.yaw,
+                    roll_speed=attitude_data.rollspeed,
+                    pitch_speed=attitude_data.pitchspeed,
+                    yaw_speed=attitude_data.yawspeed,
+                )
+
+                self.__logger.info(f"Telemetry data received: {telemetry_data}")
+                return True, telemetry_data
+
+            # Update remaining timeout
+            elapsed = time.time() - start_time
+            remaining_timeout = TELEMETRY_TIMEOUT - elapsed
+
+        # Timeout: did not receive both messages in time
+        self.__logger.error(
+            "Telemetry timeout: did not receive both ATTITUDE and LOCAL_POSITION_NED"
+        )
+        return False, None
 
 
 # =================================================================================================
diff --git a/modules/telemetry/telemetry_worker.py b/modules/telemetry/telemetry_worker.py
index ed353145..57fcced3 100644
--- a/modules/telemetry/telemetry_worker.py
+++ b/modules/telemetry/telemetry_worker.py
@@ -18,13 +18,16 @@
 # =================================================================================================
 def telemetry_worker(
     connection: mavutil.mavfile,
-    args,  # Place your own arguments here
-    # Add other necessary worker arguments here
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
-    Worker process.
+    Worker process that gathers telemetry data (position and attitude) from the drone.
 
-    args... describe what the arguments are
+    Parameters:
+        connection: MAVLink connection to the drone.
+        output_queue: Queue to output TelemetryData objects.
+        controller: WorkerController for managing pause/exit requests.
     """
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -47,8 +50,30 @@ 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("Failed to create Telemetry")
+        return
+
+    # Get Pylance to stop complaining
+    assert telemetry_instance is not None
+
+    local_logger.info("Telemetry created successfully")
 
     # Main loop: do work.
+    while not controller.is_exit_requested():
+        # Check if pause has been requested
+        controller.check_pause()
+
+        # Receive telemetry data
+        result, telemetry_data = telemetry_instance.run()
+
+        if result and telemetry_data is not None:
+            # Output the telemetry data to the queue
+            output_queue.queue.put(telemetry_data)
+            local_logger.info(f"Telemetry data sent to queue: {telemetry_data}")
+
+    local_logger.info("Telemetry worker exiting")
 
 
 # =================================================================================================
diff --git a/tests/integration/test_command.py b/tests/integration/test_command.py
index 9c1ca3c1..487e7761 100644
--- a/tests/integration/test_command.py
+++ b/tests/integration/test_command.py
@@ -35,6 +35,7 @@
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 # Add your own constants here
+QUEUE_MAX_SIZE = 50
 
 # =================================================================================================
 #                            ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -54,31 +55,66 @@ def start_drone() -> None:
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 def stop(
-    args,  # Add any necessary arguments
+    controller: worker_controller.WorkerController,
+    input_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
 ) -> None:
     """
     Stop the workers.
+
+    Parameters:
+        controller: WorkerController to signal workers to stop.
+        input_queue: Input queue to drain on shutdown.
+        output_queue: Output queue to drain on shutdown.
     """
-    pass  # Add logic to stop your worker
+    controller.request_exit()
+    input_queue.fill_and_drain_queue()
+    output_queue.fill_and_drain_queue()
 
 
 def read_queue(
-    args,  # Add any necessary arguments
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
     main_logger: logger.Logger,
 ) -> None:
     """
     Read and print the output queue.
+
+    Parameters:
+        output_queue: Queue to read command status from.
+        controller: WorkerController to check if exit was requested.
+        main_logger: Logger to log the output.
     """
-    pass  # Add logic to read from your worker's output queue and print it using the logger
+    while not controller.is_exit_requested():
+        try:
+            # Use a timeout to periodically check if exit was requested
+            status = output_queue.queue.get(timeout=0.5)
+            if status is None:
+                break
+            main_logger.info(f"Worker output: {status}")
+        except Exception:  # pylint: disable=broad-except
+            # Queue.get with timeout raises Empty exception if no data
+            continue
 
 
 def put_queue(
-    args,  # Add any necessary arguments
+    input_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    path: list,
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
     Place mocked inputs into the input queue periodically with period TELEMETRY_PERIOD.
+
+    Parameters:
+        input_queue: Queue to put telemetry data into.
+        path: List of TelemetryData objects to put into the queue.
+        controller: WorkerController to check if exit was requested.
     """
-    pass  # Add logic to place the mocked inputs into your worker's input queue periodically
+    for telemetry_data in path:
+        if controller.is_exit_requested():
+            break
+        input_queue.queue.put(telemetry_data)
+        time.sleep(TELEMETRY_PERIOD)
 
 
 # =================================================================================================
@@ -127,10 +163,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
+    input_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, QUEUE_MAX_SIZE)
+    output_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, QUEUE_MAX_SIZE)
 
     # Test cases, DO NOT EDIT!
     path = [
@@ -217,16 +257,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) + 1,
+        stop,
+        (controller, input_queue, output_queue),
+    ).start()
 
     # Put items into input queue
-    threading.Thread(target=put_queue, args=(args,)).start()
+    threading.Thread(target=put_queue, args=(input_queue, path, controller)).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,
+        input_queue,
+        output_queue,
+        controller,
     )
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
diff --git a/tests/integration/test_heartbeat_receiver.py b/tests/integration/test_heartbeat_receiver.py
index 23ff4cc5..130f676b 100644
--- a/tests/integration/test_heartbeat_receiver.py
+++ b/tests/integration/test_heartbeat_receiver.py
@@ -30,6 +30,7 @@
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 # Add your own constants here
+QUEUE_MAX_SIZE = 10
 
 # =================================================================================================
 #                            ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -49,22 +50,43 @@ def start_drone() -> None:
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 def stop(
-    args,  # Add any necessary arguments
+    controller: worker_controller.WorkerController,
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
 ) -> None:
     """
     Stop the workers.
+
+    Parameters:
+        controller: WorkerController to signal workers to stop.
+        output_queue: Queue to drain on shutdown.
     """
-    pass  # Add logic to stop your worker
+    controller.request_exit()
+    output_queue.fill_and_drain_queue()
 
 
 def read_queue(
-    args,  # Add any necessary arguments
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
     main_logger: logger.Logger,
 ) -> None:
     """
     Read and print the output queue.
+
+    Parameters:
+        output_queue: Queue to read connection status from.
+        controller: WorkerController to check if exit was requested.
+        main_logger: Logger to log the output.
     """
-    pass  # Add logic to read from your worker's output queue and print it using the logger
+    while not controller.is_exit_requested():
+        try:
+            # Use a timeout to periodically check if exit was requested
+            status = output_queue.queue.get(timeout=0.5)
+            if status is None:
+                break
+            main_logger.info(f"Worker output: {status}")
+        except Exception:  # pylint: disable=broad-except
+            # Queue.get with timeout raises Empty exception if no data
+            continue
 
 
 # =================================================================================================
@@ -113,23 +135,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
+    mp_manager = mp.Manager()
 
     # Create your queues
+    output_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, QUEUE_MAX_SIZE)
 
     # 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,),
+        (controller, output_queue),
     ).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()
 
     heartbeat_receiver_worker.heartbeat_receiver_worker(
-        # Place your own arguments here
+        connection,
+        output_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..5b75262d 100644
--- a/tests/integration/test_heartbeat_sender.py
+++ b/tests/integration/test_heartbeat_sender.py
@@ -45,12 +45,15 @@ def start_drone() -> None:
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 def stop(
-    args,  # Add any necessary arguments
+    controller: worker_controller.WorkerController,
 ) -> None:
     """
     Stop the workers.
+
+    Parameters:
+        controller: WorkerController to signal workers to stop.
     """
-    pass  # Add logic to stop your worker
+    controller.request_exit()
 
 
 # =================================================================================================
@@ -93,12 +96,15 @@ 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()
+    # Account for initial heartbeat consumed by wait_heartbeat() and worker initialization time
+    threading.Timer(HEARTBEAT_PERIOD * (NUM_TRIALS + 1), stop, (controller,)).start()
 
     heartbeat_sender_worker.heartbeat_sender_worker(
-        # Place your own arguments here
+        connection,
+        controller,
     )
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
diff --git a/tests/integration/test_telemetry.py b/tests/integration/test_telemetry.py
index 79f9e497..dfbca967 100644
--- a/tests/integration/test_telemetry.py
+++ b/tests/integration/test_telemetry.py
@@ -28,6 +28,7 @@
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 # Add your own constants here
+QUEUE_MAX_SIZE = 10
 
 # =================================================================================================
 #                            ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑
@@ -47,22 +48,43 @@ def start_drone() -> None:
 #                            ↓ BOOTCAMPERS MODIFY BELOW THIS COMMENT ↓
 # =================================================================================================
 def stop(
-    args,  # Add any necessary arguments
+    controller: worker_controller.WorkerController,
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
 ) -> None:
     """
     Stop the workers.
+
+    Parameters:
+        controller: WorkerController to signal workers to stop.
+        output_queue: Queue to drain on shutdown.
     """
-    pass  # Add logic to stop your worker
+    controller.request_exit()
+    output_queue.fill_and_drain_queue()
 
 
 def read_queue(
-    args,  # Add any necessary arguments
+    output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    controller: worker_controller.WorkerController,
     main_logger: logger.Logger,
 ) -> None:
     """
     Read and print the output queue.
+
+    Parameters:
+        output_queue: Queue to read telemetry data from.
+        controller: WorkerController to check if exit was requested.
+        main_logger: Logger to log the output.
     """
-    pass  # Add logic to read from your worker's output queue and print it using the logger
+    while not controller.is_exit_requested():
+        try:
+            # Use a timeout to periodically check if exit was requested
+            telemetry_data = output_queue.queue.get(timeout=0.5)
+            if telemetry_data is None:
+                break
+            main_logger.info(f"Worker output: {telemetry_data}")
+        except Exception:  # pylint: disable=broad-except
+            # Queue.get with timeout raises Empty exception if no data
+            continue
 
 
 # =================================================================================================
@@ -111,19 +133,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
+    mp_manager = mp.Manager()
 
     # Create your queues
+    output_queue = queue_proxy_wrapper.QueueProxyWrapper(mp_manager, QUEUE_MAX_SIZE)
 
     # 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,
+        (controller, output_queue),
+    ).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()
 
     telemetry_worker.telemetry_worker(
-        # Put your own arguments here
+        connection,
+        output_queue,
+        controller,
     )
     # =============================================================================================
     #                          ↑ BOOTCAMPERS MODIFY ABOVE THIS COMMENT ↑