From 90606981159daa44715868ee9d6109ba28e79531 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Wed, 17 Sep 2025 23:07:56 -0400
Subject: [PATCH 01/16] Adds multithreading to hitl modules

---
 modules/hitl/camera_emulator.py   | 23 +++++++++++
 modules/hitl/hitl_base.py         | 69 ++++++++++++++++++++++++++++++-
 modules/hitl/position_emulator.py | 20 ++++++++-
 3 files changed, 110 insertions(+), 2 deletions(-)

diff --git a/modules/hitl/camera_emulator.py b/modules/hitl/camera_emulator.py
index bfd20c3..312006e 100644
--- a/modules/hitl/camera_emulator.py
+++ b/modules/hitl/camera_emulator.py
@@ -6,6 +6,7 @@
 """
 
 import os
+import time
 import pyvirtualcam
 import cv2
 
@@ -66,10 +67,32 @@ def __init__(
         self.__image_paths: "list[str]" = []
         self.__current_frame = None
         self.__image_index = 0
+        self.__next_image_time = time.time() + 1.0
 
         self.__get_images()
         self.update_current_image()
 
+    def periodic(self) -> None:
+        """
+        Periodic function.
+        """
+        try:
+            # Send frame and pace to target FPS
+            self.send_frame()
+            self.sleep_until_next_frame()
+
+            now = time.time()
+            if now >= self.__next_image_time:
+                # Cycle image once per second
+                try:
+                    self.next_image()
+                    self.update_current_image()
+                except Exception as exc:  # pylint: disable=broad-except
+                    print(f"HITL camera image update error: {exc}")
+                self.__next_image_time = now + 1.0
+        except Exception as exc:  # pylint: disable=broad-except
+            print(f"HITL camera periodic error: {exc}")
+
     def send_frame(self) -> None:
         """
         sends a new frame to virtual camera, should be called in a loop
diff --git a/modules/hitl/hitl_base.py b/modules/hitl/hitl_base.py
index d6e3982..af80ee0 100644
--- a/modules/hitl/hitl_base.py
+++ b/modules/hitl/hitl_base.py
@@ -3,6 +3,7 @@
 """
 
 import time
+from threading import Event, Thread
 from modules.hitl.position_emulator import PositionEmulator
 from modules.hitl.camera_emulator import CameraEmulator
 from ..mavlink import dronekit
@@ -47,7 +48,6 @@ def create(
 
         if position_module:
             result, position_emulator = PositionEmulator.create(drone)
-            position_emulator.inject_position()  # Inject initial position
             if not result:
                 return False, None
 
@@ -81,6 +81,73 @@ def __init__(
         self.position_emulator = position_emulator
         self.camera_emulator = camera_emulator
 
+        self._stop_event: Event | None = None
+        self._threads: list[Thread] = []
+
+    def start(self) -> None:
+        """
+        Start HITL module threads.
+        """
+        if self._stop_event is not None:
+            return
+
+        self._stop_event = Event()
+        self._threads = []
+
+        if self.position_emulator is not None:
+            t = Thread(target=self.run_position, name="HITL-Position", daemon=True)
+            self._threads.append(t)
+            t.start()
+
+        if self.camera_emulator is not None:
+            t = Thread(target=self.run_camera, name="HITL-Camera", daemon=True)
+            self._threads.append(t)
+            t.start()
+
+    def shutdown(self, join_timeout: float | None = 5.0) -> None:
+        """
+        Signal threads to stop and join them.
+        """
+        if self._stop_event is None:
+            return
+
+        self._stop_event.set()
+
+        for t in self._threads:
+            if t.is_alive():
+                t.join(timeout=join_timeout)
+
+        self._threads.clear()
+        self._stop_event = None
+
+    def __del__(self) -> None:
+        """
+        Best-effort cleanup when HITL object is destroyed.
+        Ensures threads are stopped and the drone connection is closed.
+        """
+        try:
+            self.shutdown()
+        except Exception:  # pylint: disable=broad-except
+            pass
+
+    def run_position(self) -> None:
+        assert self._stop_event is not None
+        while not self._stop_event.is_set():
+            try:
+                self.position_emulator.periodic()
+            except Exception as exc:  # pylint: disable=broad-except
+                print(f"HITL position thread error: {exc}")
+            # keep it light
+            time.sleep(1.0)
+
+    def run_camera(self) -> None:
+        assert self._stop_event is not None
+        while not self._stop_event.is_set():
+            try:
+                self.camera_emulator.periodic()
+            except Exception as exc:  # pylint: disable=broad-except
+                print(f"HITL camera thread error: {exc}")
+
     def set_inject_position(self) -> None:
         """
         Set the position to inject into the drone.
diff --git a/modules/hitl/position_emulator.py b/modules/hitl/position_emulator.py
index 50c5ac7..9e5831e 100644
--- a/modules/hitl/position_emulator.py
+++ b/modules/hitl/position_emulator.py
@@ -32,8 +32,27 @@ def __init__(self, class_private_create_key: object, drone: dronekit.Vehicle) ->
         """
         assert class_private_create_key is PositionEmulator.__create_key, "Use create() method"
 
+        self.target_position = (43.43405014107003, -80.57898027451816, 373.0)  # lat, lon, alt
+
         self.drone = drone
 
+    def set_target_position(self, latitude: float, longitude: float, altitude: float) -> None:
+        """
+        Sets the target position.
+
+        Args:
+            latitude: Latitude in degrees.
+            longitude: Longitude in degrees.
+            altitude: Altitude in meters.
+        """
+        self.target_position = (latitude, longitude, altitude)
+
+    def periodic(self) -> None:
+        """
+        Periodic function.
+        """
+        self.inject_position(self.target_position[0], self.target_position[1], self.target_position[2])
+
     def inject_position(
         self,
         latitude: float = 43.43405014107003,
@@ -68,7 +87,6 @@ def inject_position(
             10,  # satellites_visible
             0,  # yaw (deg*100)
         ]
-        print("Packing values:", values)
         gps_input_msg = self.drone.message_factory.gps_input_encode(*values)
         self.drone.send_mavlink(gps_input_msg)
         self.drone.flush()

From 5af546e68c6a5070269aa55c2c02d1da33d8cc8a Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Wed, 17 Sep 2025 23:09:16 -0400
Subject: [PATCH 02/16] Adds flight controller start method for hitl threading

---
 modules/mavlink/flight_controller.py | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/modules/mavlink/flight_controller.py b/modules/mavlink/flight_controller.py
index 9c23490..f62d2a0 100644
--- a/modules/mavlink/flight_controller.py
+++ b/modules/mavlink/flight_controller.py
@@ -211,6 +211,7 @@ def create(
         position_module: bool = False,
         camera_module: bool = False,
         images_path: str | None = None,
+        positions_path: str | None = None,
     ) -> "tuple[bool, FlightController | None]":
         """
         address: TCP address or serial port of the drone (e.g. "tcp:127.0.0.1:14550").
@@ -229,7 +230,10 @@ def create(
             success, hitl_instance = hitl_base.HITL.create(
                 drone, hitl_enabled, position_module, camera_module, images_path
             )
-            if not success:
+            if success:
+                if hitl_enabled and hitl_instance is not None:
+                    hitl_instance.start()
+            else:
                 print("Error creating HITL module")
 
         except dronekit.TimeoutError:

From 7670062fc5634c3798c4eb0293f334d9fc879e33 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Wed, 17 Sep 2025 23:32:07 -0400
Subject: [PATCH 03/16] Removes positions json path

---
 modules/mavlink/flight_controller.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/modules/mavlink/flight_controller.py b/modules/mavlink/flight_controller.py
index f62d2a0..7d46d70 100644
--- a/modules/mavlink/flight_controller.py
+++ b/modules/mavlink/flight_controller.py
@@ -211,7 +211,6 @@ def create(
         position_module: bool = False,
         camera_module: bool = False,
         images_path: str | None = None,
-        positions_path: str | None = None,
     ) -> "tuple[bool, FlightController | None]":
         """
         address: TCP address or serial port of the drone (e.g. "tcp:127.0.0.1:14550").

From 2bf98af552286abd0b72466c448099c0c9c5a1bb Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Wed, 17 Sep 2025 23:40:50 -0400
Subject: [PATCH 04/16] Adds proper position listening for mission waypoints in
 hitl

---
 modules/hitl/hitl_base.py                | 24 ---------------
 modules/hitl/position_emulator.py        | 24 ++++++++++++++-
 tests/integration/hitl/test_threading.py | 38 ++++++++++++++++++++++++
 3 files changed, 61 insertions(+), 25 deletions(-)
 create mode 100644 tests/integration/hitl/test_threading.py

diff --git a/modules/hitl/hitl_base.py b/modules/hitl/hitl_base.py
index af80ee0..68a6add 100644
--- a/modules/hitl/hitl_base.py
+++ b/modules/hitl/hitl_base.py
@@ -147,27 +147,3 @@ def run_camera(self) -> None:
                 self.camera_emulator.periodic()
             except Exception as exc:  # pylint: disable=broad-except
                 print(f"HITL camera thread error: {exc}")
-
-    def set_inject_position(self) -> None:
-        """
-        Set the position to inject into the drone.
-        Print out a message if position emulator is not enabled.
-
-        """
-        if self.position_emulator is None:
-            print("Position emulator is not enabled.")
-            return
-        # pylint: disable=protected-access
-        position_target = self.drone._master.recv_match(...)
-        # pylint: enable=protected-access
-        if position_target:
-            latitude = position_target.lat_int / 1e7
-            longitude = position_target.lon_int / 1e7
-            altitude = position_target.alt
-
-            self.position_emulator.inject_position(latitude, longitude, altitude)
-            print(f"Injected position: lat={latitude}, lon={longitude}, alt={altitude}")
-        else:
-            print("No POSITION_TARGET_GLOBAL_INT message received.")
-
-        time.sleep(3)
diff --git a/modules/hitl/position_emulator.py b/modules/hitl/position_emulator.py
index 9e5831e..140b2d7 100644
--- a/modules/hitl/position_emulator.py
+++ b/modules/hitl/position_emulator.py
@@ -38,7 +38,7 @@ def __init__(self, class_private_create_key: object, drone: dronekit.Vehicle) ->
 
     def set_target_position(self, latitude: float, longitude: float, altitude: float) -> None:
         """
-        Sets the target position.
+        Sets the target position manually (currently a fallback if Ardupilot target doesnt work).
 
         Args:
             latitude: Latitude in degrees.
@@ -47,10 +47,32 @@ def set_target_position(self, latitude: float, longitude: float, altitude: float
         """
         self.target_position = (latitude, longitude, altitude)
 
+    def get_target_position(self) -> tuple[float, float, float]:
+        """
+        Gets the target position from the Ardupilot target.
+
+        Returns:
+            Target position as (latitude, longitude, altitude).
+        """
+        # pylint: disable=protected-access
+        position_target = self.drone._master.recv_match(...)
+        # pylint: enable=protected-access
+        if position_target:
+            latitude = position_target.lat_int / 1e7
+            longitude = position_target.lon_int / 1e7
+            altitude = position_target.alt
+        else:
+            print("No POSITION_TARGET_GLOBAL_INT message received.")
+
+        return (latitude, longitude, altitude) if position_target else self.target_position
+
     def periodic(self) -> None:
         """
         Periodic function.
         """
+
+        self.target_position = self.get_target_position()
+
         self.inject_position(self.target_position[0], self.target_position[1], self.target_position[2])
 
     def inject_position(
diff --git a/tests/integration/hitl/test_threading.py b/tests/integration/hitl/test_threading.py
new file mode 100644
index 0000000..3670eed
--- /dev/null
+++ b/tests/integration/hitl/test_threading.py
@@ -0,0 +1,38 @@
+"""
+Tests the threading behavior of the HITL module (gps and camera modules).
+"""
+
+PIXHAWK_ADDRESS = "tcp:localhost:5762"
+
+import os
+from time import time
+from modules.mavlink import flight_controller
+
+def main() -> int:
+    """
+    Main function.
+    """
+    images_folder_path = os.path.join("tests", "integration", "camera_emulator", "images")
+
+    result, controller = flight_controller.FlightController.create(
+        PIXHAWK_ADDRESS, 57600, True, True, True, images_folder_path
+    )
+    if not result:
+        print("Failed to create flight controller")
+        return -1
+
+    controller.set_flight_mode("AUTO")
+
+    controller.insert_waypoint(0, 40, -40, 300)
+
+    time.sleep(20)
+
+    return 0
+
+
+if __name__ == "__main__":
+    result_main = main()
+    if result_main != 0:
+        print(f"ERROR: Status code: {result_main}")
+
+    print("Done!")

From 0f606b132aa984449ee70cab0fd2f021cc3dc920 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Wed, 17 Sep 2025 23:52:41 -0400
Subject: [PATCH 05/16] Removes camera part of threading test

---
 tests/integration/hitl/test_threading.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/integration/hitl/test_threading.py b/tests/integration/hitl/test_threading.py
index 3670eed..1e019c4 100644
--- a/tests/integration/hitl/test_threading.py
+++ b/tests/integration/hitl/test_threading.py
@@ -15,7 +15,7 @@ def main() -> int:
     images_folder_path = os.path.join("tests", "integration", "camera_emulator", "images")
 
     result, controller = flight_controller.FlightController.create(
-        PIXHAWK_ADDRESS, 57600, True, True, True, images_folder_path
+        PIXHAWK_ADDRESS, 57600, True, True, False # True, images_folder_path
     )
     if not result:
         print("Failed to create flight controller")

From af59f0f511e7c47971ed3e7b2a01b35de1bb0f3a Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Wed, 17 Sep 2025 23:56:01 -0400
Subject: [PATCH 06/16] Actually removes camera from test

---
 tests/integration/hitl/test_threading.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/integration/hitl/test_threading.py b/tests/integration/hitl/test_threading.py
index 1e019c4..6c76b04 100644
--- a/tests/integration/hitl/test_threading.py
+++ b/tests/integration/hitl/test_threading.py
@@ -15,7 +15,7 @@ def main() -> int:
     images_folder_path = os.path.join("tests", "integration", "camera_emulator", "images")
 
     result, controller = flight_controller.FlightController.create(
-        PIXHAWK_ADDRESS, 57600, True, True, False # True, images_folder_path
+        PIXHAWK_ADDRESS, 57600, True, True # True, images_folder_path
     )
     if not result:
         print("Failed to create flight controller")

From 0cb60ab7ffbb3ff066066d9d9252dc55eec51924 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Thu, 18 Sep 2025 01:32:59 -0400
Subject: [PATCH 07/16] Fixes time sleep function in hitl threading

---
 tests/integration/hitl/test_threading.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/integration/hitl/test_threading.py b/tests/integration/hitl/test_threading.py
index 6c76b04..b5d3dd1 100644
--- a/tests/integration/hitl/test_threading.py
+++ b/tests/integration/hitl/test_threading.py
@@ -5,7 +5,7 @@
 PIXHAWK_ADDRESS = "tcp:localhost:5762"
 
 import os
-from time import time
+import time
 from modules.mavlink import flight_controller
 
 def main() -> int:

From b2af8522844246d3f4ac30db2c20ec7ec61b1ef9 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Thu, 18 Sep 2025 01:39:51 -0400
Subject: [PATCH 08/16] Makes gps speed faster

---
 modules/hitl/hitl_base.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/modules/hitl/hitl_base.py b/modules/hitl/hitl_base.py
index 68a6add..5f66833 100644
--- a/modules/hitl/hitl_base.py
+++ b/modules/hitl/hitl_base.py
@@ -137,8 +137,7 @@ def run_position(self) -> None:
                 self.position_emulator.periodic()
             except Exception as exc:  # pylint: disable=broad-except
                 print(f"HITL position thread error: {exc}")
-            # keep it light
-            time.sleep(1.0)
+            time.sleep(0.02)
 
     def run_camera(self) -> None:
         assert self._stop_event is not None

From c0d433142a01a71456703c2d05264a222bb8a6f4 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Thu, 18 Sep 2025 01:45:30 -0400
Subject: [PATCH 09/16] Moves position frequency control to position module

---
 modules/hitl/hitl_base.py         | 1 -
 modules/hitl/position_emulator.py | 2 ++
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/modules/hitl/hitl_base.py b/modules/hitl/hitl_base.py
index 5f66833..fc0bfaf 100644
--- a/modules/hitl/hitl_base.py
+++ b/modules/hitl/hitl_base.py
@@ -137,7 +137,6 @@ def run_position(self) -> None:
                 self.position_emulator.periodic()
             except Exception as exc:  # pylint: disable=broad-except
                 print(f"HITL position thread error: {exc}")
-            time.sleep(0.02)
 
     def run_camera(self) -> None:
         assert self._stop_event is not None
diff --git a/modules/hitl/position_emulator.py b/modules/hitl/position_emulator.py
index 140b2d7..ea32460 100644
--- a/modules/hitl/position_emulator.py
+++ b/modules/hitl/position_emulator.py
@@ -75,6 +75,8 @@ def periodic(self) -> None:
 
         self.inject_position(self.target_position[0], self.target_position[1], self.target_position[2])
 
+        time.sleep(0.1) # 10 Hz
+
     def inject_position(
         self,
         latitude: float = 43.43405014107003,

From da74d8640f7b7134b5e2cc36779afa138e51cb91 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Thu, 18 Sep 2025 01:54:56 -0400
Subject: [PATCH 10/16] Changes threading test order

---
 tests/integration/hitl/test_threading.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/tests/integration/hitl/test_threading.py b/tests/integration/hitl/test_threading.py
index b5d3dd1..2acb887 100644
--- a/tests/integration/hitl/test_threading.py
+++ b/tests/integration/hitl/test_threading.py
@@ -21,11 +21,13 @@ def main() -> int:
         print("Failed to create flight controller")
         return -1
 
+    time.sleep(1)
+
     controller.set_flight_mode("AUTO")
 
     controller.insert_waypoint(0, 40, -40, 300)
 
-    time.sleep(20)
+    time.sleep(10)
 
     return 0
 

From a25cbc9252f2ee62b372de09763c8f99a85a81f4 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Thu, 18 Sep 2025 02:11:27 -0400
Subject: [PATCH 11/16] Fixes linting issues

---
 modules/hitl/hitl_base.py                |  8 ++++++++
 modules/hitl/position_emulator.py        | 24 +++++++++++++++++-------
 tests/integration/hitl/test_threading.py |  3 ++-
 3 files changed, 27 insertions(+), 8 deletions(-)

diff --git a/modules/hitl/hitl_base.py b/modules/hitl/hitl_base.py
index fc0bfaf..5c40b3d 100644
--- a/modules/hitl/hitl_base.py
+++ b/modules/hitl/hitl_base.py
@@ -131,17 +131,25 @@ def __del__(self) -> None:
             pass
 
     def run_position(self) -> None:
+        """
+        Runs the position emulator periodic function in a loop.
+        """
         assert self._stop_event is not None
         while not self._stop_event.is_set():
             try:
                 self.position_emulator.periodic()
             except Exception as exc:  # pylint: disable=broad-except
                 print(f"HITL position thread error: {exc}")
+                time.sleep(0.1)
 
     def run_camera(self) -> None:
+        """
+        Runs the camera emulator periodic function in a loop.
+        """
         assert self._stop_event is not None
         while not self._stop_event.is_set():
             try:
                 self.camera_emulator.periodic()
             except Exception as exc:  # pylint: disable=broad-except
                 print(f"HITL camera thread error: {exc}")
+                time.sleep(0.1)
diff --git a/modules/hitl/position_emulator.py b/modules/hitl/position_emulator.py
index ea32460..104dd7c 100644
--- a/modules/hitl/position_emulator.py
+++ b/modules/hitl/position_emulator.py
@@ -55,16 +55,26 @@ def get_target_position(self) -> tuple[float, float, float]:
             Target position as (latitude, longitude, altitude).
         """
         # pylint: disable=protected-access
-        position_target = self.drone._master.recv_match(...)
+        position_target = None
+        try:
+            # Poll non-blocking for the latest global position target from the FCU
+            position_target = self.drone._master.recv_match(
+                type="POSITION_TARGET_GLOBAL_INT", blocking=False
+            )
+        except Exception as exc:  # pylint: disable=broad-except
+            print(f"HITL get_target_position recv_match error: {exc}")
+            position_target = None
         # pylint: enable=protected-access
+
         if position_target:
             latitude = position_target.lat_int / 1e7
             longitude = position_target.lon_int / 1e7
             altitude = position_target.alt
-        else:
-            print("No POSITION_TARGET_GLOBAL_INT message received.")
+            return (latitude, longitude, altitude)
+
+        print("No POSITION_TARGET_GLOBAL_INT message received.")
 
-        return (latitude, longitude, altitude) if position_target else self.target_position
+        return self.target_position
 
     def periodic(self) -> None:
         """
@@ -73,9 +83,9 @@ def periodic(self) -> None:
 
         self.target_position = self.get_target_position()
 
-        self.inject_position(self.target_position[0], self.target_position[1], self.target_position[2])
-
-        time.sleep(0.1) # 10 Hz
+        self.inject_position(
+            self.target_position[0], self.target_position[1], self.target_position[2]
+        )
 
     def inject_position(
         self,
diff --git a/tests/integration/hitl/test_threading.py b/tests/integration/hitl/test_threading.py
index 2acb887..d03154a 100644
--- a/tests/integration/hitl/test_threading.py
+++ b/tests/integration/hitl/test_threading.py
@@ -8,6 +8,7 @@
 import time
 from modules.mavlink import flight_controller
 
+
 def main() -> int:
     """
     Main function.
@@ -15,7 +16,7 @@ def main() -> int:
     images_folder_path = os.path.join("tests", "integration", "camera_emulator", "images")
 
     result, controller = flight_controller.FlightController.create(
-        PIXHAWK_ADDRESS, 57600, True, True # True, images_folder_path
+        PIXHAWK_ADDRESS, 57600, True, True  # True, images_folder_path
     )
     if not result:
         print("Failed to create flight controller")

From 665224bf6fd14a8040644df8ef6edd83852de4a9 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Mon, 22 Sep 2025 16:36:29 -0400
Subject: [PATCH 12/16] Fixes port for physical test

---
 modules/hitl/position_emulator.py        | 2 +-
 tests/integration/hitl/test_threading.py | 6 ++++--
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/modules/hitl/position_emulator.py b/modules/hitl/position_emulator.py
index 104dd7c..bc33abe 100644
--- a/modules/hitl/position_emulator.py
+++ b/modules/hitl/position_emulator.py
@@ -72,7 +72,7 @@ def get_target_position(self) -> tuple[float, float, float]:
             altitude = position_target.alt
             return (latitude, longitude, altitude)
 
-        print("No POSITION_TARGET_GLOBAL_INT message received.")
+        # print("No POSITION_TARGET_GLOBAL_INT message received.")
 
         return self.target_position
 
diff --git a/tests/integration/hitl/test_threading.py b/tests/integration/hitl/test_threading.py
index d03154a..9b4a222 100644
--- a/tests/integration/hitl/test_threading.py
+++ b/tests/integration/hitl/test_threading.py
@@ -2,7 +2,9 @@
 Tests the threading behavior of the HITL module (gps and camera modules).
 """
 
-PIXHAWK_ADDRESS = "tcp:localhost:5762"
+# Physical connection to Pixhawk: /dev/ttyAMA0
+# Simulated connection to Pixhawk: tcp:localhost:5762
+PIXHAWK_ADDRESS = "/dev/ttyAMA0"
 
 import os
 import time
@@ -16,7 +18,7 @@ def main() -> int:
     images_folder_path = os.path.join("tests", "integration", "camera_emulator", "images")
 
     result, controller = flight_controller.FlightController.create(
-        PIXHAWK_ADDRESS, 57600, True, True  # True, images_folder_path
+        PIXHAWK_ADDRESS, 57600, True, True, True, images_folder_path
     )
     if not result:
         print("Failed to create flight controller")

From ae2958ddf61129271ffab91fe18a9313be30c5d3 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Tue, 23 Sep 2025 18:43:39 -0400
Subject: [PATCH 13/16] Removes auto mode and waypoint injection from threading
 test

---
 tests/integration/hitl/test_threading.py | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/tests/integration/hitl/test_threading.py b/tests/integration/hitl/test_threading.py
index 9b4a222..1a17542 100644
--- a/tests/integration/hitl/test_threading.py
+++ b/tests/integration/hitl/test_threading.py
@@ -24,12 +24,6 @@ def main() -> int:
         print("Failed to create flight controller")
         return -1
 
-    time.sleep(1)
-
-    controller.set_flight_mode("AUTO")
-
-    controller.insert_waypoint(0, 40, -40, 300)
-
     time.sleep(10)
 
     return 0

From 1149b62a8b4a1a332f4955626a81295e13d5b1a0 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Tue, 23 Sep 2025 19:41:49 -0400
Subject: [PATCH 14/16] Removes stuff

---
 modules/hitl/position_emulator.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/modules/hitl/position_emulator.py b/modules/hitl/position_emulator.py
index bc33abe..eb3c3cb 100644
--- a/modules/hitl/position_emulator.py
+++ b/modules/hitl/position_emulator.py
@@ -57,7 +57,6 @@ def get_target_position(self) -> tuple[float, float, float]:
         # pylint: disable=protected-access
         position_target = None
         try:
-            # Poll non-blocking for the latest global position target from the FCU
             position_target = self.drone._master.recv_match(
                 type="POSITION_TARGET_GLOBAL_INT", blocking=False
             )
@@ -72,6 +71,7 @@ def get_target_position(self) -> tuple[float, float, float]:
             altitude = position_target.alt
             return (latitude, longitude, altitude)
 
+        # Optionally log if no message received
         # print("No POSITION_TARGET_GLOBAL_INT message received.")
 
         return self.target_position

From d5c26ed001aee46bd226f41613f149d3823f3909 Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Thu, 25 Sep 2025 14:43:27 -0400
Subject: [PATCH 15/16] Adds time between images as a parameter

---
 modules/hitl/camera_emulator.py | 25 ++++++++++++++++++++-----
 1 file changed, 20 insertions(+), 5 deletions(-)

diff --git a/modules/hitl/camera_emulator.py b/modules/hitl/camera_emulator.py
index 312006e..1064ed7 100644
--- a/modules/hitl/camera_emulator.py
+++ b/modules/hitl/camera_emulator.py
@@ -23,18 +23,32 @@ class CameraEmulator:
     __create_key = object()
 
     @classmethod
-    def create(cls, images_path: str) -> "tuple[True, CameraEmulator] | tuple[False, None]":
+    def create(cls, images_path: str, time_between_images: float = 1.0) -> "tuple[True, CameraEmulator] | tuple[False, None]":
         """
         Setup camera emulator.
 
         Args:
             images_path: Path to the directory containing images for the camera emulator. Cycles through these images to simulate camera input (every 1 second).
+            time_between_images: Time in seconds between image changes.
 
         Returns:
             Success, CameraEmulator instance.
         """
 
         if not isinstance(images_path, str):
+            print("Images path is not a string")
+            return False, None
+        
+        if not os.path.isdir(images_path):
+            print("Images path is not a valid directory")
+            return False, None
+        
+        if not isinstance(time_between_images, (int, float)):
+            print("Time between images is not a number")
+            return False, None
+
+        if time_between_images <= 0:
+            print("Time between images must be positive")
             return False, None
 
         try:
@@ -52,10 +66,10 @@ def create(cls, images_path: str) -> "tuple[True, CameraEmulator] | tuple[False,
         if virtual_camera_instance is None:
             return False, None
 
-        return True, CameraEmulator(cls.__create_key, images_path, virtual_camera_instance)
+        return True, CameraEmulator(cls.__create_key, images_path, time_between_images, virtual_camera_instance)
 
     def __init__(
-        self, class_private_create_key: object, images_path: str, virtual_camera: pyvirtualcam
+        self, class_private_create_key: object, images_path: str, time_between_images: float, virtual_camera: pyvirtualcam
     ) -> None:
         """
         Private constructor, use create() method.
@@ -67,7 +81,8 @@ def __init__(
         self.__image_paths: "list[str]" = []
         self.__current_frame = None
         self.__image_index = 0
-        self.__next_image_time = time.time() + 1.0
+        self.__next_image_time = time.time() + time_between_images
+        self.__time_between_images = time_between_images
 
         self.__get_images()
         self.update_current_image()
@@ -89,7 +104,7 @@ def periodic(self) -> None:
                     self.update_current_image()
                 except Exception as exc:  # pylint: disable=broad-except
                     print(f"HITL camera image update error: {exc}")
-                self.__next_image_time = now + 1.0
+                self.__next_image_time = now + self.__time_between_images
         except Exception as exc:  # pylint: disable=broad-except
             print(f"HITL camera periodic error: {exc}")
 

From 7b2295f7efc5e4a7ac2be489f42be8e60f893aac Mon Sep 17 00:00:00 2001
From: honzikschenk <nojschenk@gmail.com>
Date: Thu, 25 Sep 2025 14:44:07 -0400
Subject: [PATCH 16/16] Format fix

---
 modules/hitl/camera_emulator.py | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/modules/hitl/camera_emulator.py b/modules/hitl/camera_emulator.py
index 1064ed7..07e82a8 100644
--- a/modules/hitl/camera_emulator.py
+++ b/modules/hitl/camera_emulator.py
@@ -23,7 +23,9 @@ class CameraEmulator:
     __create_key = object()
 
     @classmethod
-    def create(cls, images_path: str, time_between_images: float = 1.0) -> "tuple[True, CameraEmulator] | tuple[False, None]":
+    def create(
+        cls, images_path: str, time_between_images: float = 1.0
+    ) -> "tuple[True, CameraEmulator] | tuple[False, None]":
         """
         Setup camera emulator.
 
@@ -38,11 +40,11 @@ def create(cls, images_path: str, time_between_images: float = 1.0) -> "tuple[Tr
         if not isinstance(images_path, str):
             print("Images path is not a string")
             return False, None
-        
+
         if not os.path.isdir(images_path):
             print("Images path is not a valid directory")
             return False, None
-        
+
         if not isinstance(time_between_images, (int, float)):
             print("Time between images is not a number")
             return False, None
@@ -66,10 +68,16 @@ def create(cls, images_path: str, time_between_images: float = 1.0) -> "tuple[Tr
         if virtual_camera_instance is None:
             return False, None
 
-        return True, CameraEmulator(cls.__create_key, images_path, time_between_images, virtual_camera_instance)
+        return True, CameraEmulator(
+            cls.__create_key, images_path, time_between_images, virtual_camera_instance
+        )
 
     def __init__(
-        self, class_private_create_key: object, images_path: str, time_between_images: float, virtual_camera: pyvirtualcam
+        self,
+        class_private_create_key: object,
+        images_path: str,
+        time_between_images: float,
+        virtual_camera: pyvirtualcam,
     ) -> None:
         """
         Private constructor, use create() method.