Make GPS move over time

modules/hitl/camera_emulator.py

@@ -11,7 +11,7 @@
 import cv2
 
 IMAGE_SIZE = (720, 480)
-IMAGE_FORMATS = (".png", ".jpeg", "jpg")
+IMAGE_FORMATS = (".png", ".jpeg", ".jpg")
 CAMERA_FPS = 30
 
 

modules/hitl/hitl_base.py

@@ -26,6 +26,7 @@ def create(
         position_module: bool,
         camera_module: bool,
         images_path: str | None = None,
+        movement_speed: float = 5.0,
     ) -> "tuple[True, HITL] | tuple[False, None]":
         """
         Factory method to create a HITL instance.
@@ -36,6 +37,7 @@ def create(
             position_module: Boolean indicating if the position module is enabled.
             camera_module: Boolean indicating if the camera module is enabled.
             images_path: Optional path to the images directory for the camera emulator.
+            movement_speed: Speed of drone movement in m/s for position interpolation (default: 5.0).
 
         Returns:
             Success, HITL instance | None.
@@ -47,7 +49,7 @@ def create(
             return True, HITL(cls.__create_key, drone, None, None)
 
         if position_module:
-            result, position_emulator = PositionEmulator.create(drone)
+            result, position_emulator = PositionEmulator.create(drone, movement_speed)
             if not result:
                 return False, None
 

modules/hitl/position_emulator.py

@@ -3,6 +3,7 @@
 """
 
 import time
+import math
 from ..mavlink import dronekit
 
 
@@ -15,24 +16,34 @@ class PositionEmulator:
 
     @classmethod
     def create(
-        cls, drone: dronekit.Vehicle
+        cls, drone: dronekit.Vehicle, movement_speed: float = 5.0
     ) -> "tuple[True, PositionEmulator] | tuple[False, None]":
         """
         Setup position emulator.
 
+        Args:
+            drone: The dronekit instance to use for sending MAVLink messages.
+            movement_speed: Speed of drone movement in m/s, default is 5.0.
+
         Returns:
             Success, PositionEmulator instance.
         """
 
-        return True, PositionEmulator(cls.__create_key, drone)
+        return True, PositionEmulator(cls.__create_key, drone, movement_speed)
 
-    def __init__(self, class_private_create_key: object, drone: dronekit.Vehicle) -> None:
+    def __init__(
+        self, class_private_create_key: object, drone: dronekit.Vehicle, movement_speed: float = 5.0
+    ) -> None:
         """
         Private constructor, use create() method.
         """
         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.current_position = (43.43405014107003, -80.57898027451816, 373.0)  # lat, lon, alt
+        self.movement_speed = movement_speed  # m/s
+        self.last_update_time = time.time()
+        self.waypoint_position = None
 
         self.drone = drone
 
@@ -47,12 +58,26 @@ def set_target_position(self, latitude: float, longitude: float, altitude: float
         """
         self.target_position = (latitude, longitude, altitude)
 
+    def set_waypoint_position(self, latitude: float, longitude: float, altitude: float) -> None:
+        """
+        Manually sets a waypoint for the emulator to move towards.
+
+        Args:
+            latitude: Latitude in degrees.
+            longitude: Longitude in degrees.
+            altitude: Altitude in meters.
+        """
+        self.waypoint_position = (latitude, longitude, altitude)
+        print(
+            f"HITL Position: Manual waypoint set to {latitude:.6f}, {longitude:.6f}, {altitude:.1f}m"
+        )
+
     def get_target_position(self) -> tuple[float, float, float]:
         """
-        Gets the target position from the Ardupilot target.
+        Gets the target position from the Ardupilot target and stores it as waypoint.
 
         Returns:
-            Target position as (latitude, longitude, altitude).
+            Current target position (not the waypoint).
         """
         # pylint: disable=protected-access
         position_target = None
@@ -69,24 +94,115 @@ def get_target_position(self) -> tuple[float, float, float]:
             latitude = position_target.lat_int / 1e7
             longitude = position_target.lon_int / 1e7
             altitude = position_target.alt
-            return (latitude, longitude, altitude)
 
-        # Optionally log if no message received
-        # print("No POSITION_TARGET_GLOBAL_INT message received.")
+            self.waypoint_position = (latitude, longitude, altitude)
 
         return self.target_position
 
+    def calculate_distance(
+        self, pos1: tuple[float, float, float], pos2: tuple[float, float, float]
+    ) -> float:
+        """
+        Calculate distance between two GPS coordinates using Haversine formula.
+
+        Args:
+            pos1: First position (lat, lon, alt)
+            pos2: Second position (lat, lon, alt)
+
+        Returns:
+            Distance in meters
+        """
+        lat1, lon1, alt1 = pos1
+        lat2, lon2, alt2 = pos2
+
+        # Convert to radians
+        lat1_rad = math.radians(lat1)
+        lon1_rad = math.radians(lon1)
+        lat2_rad = math.radians(lat2)
+        lon2_rad = math.radians(lon2)
+
+        # Haversine formula for horizontal distance
+        dlat = lat2_rad - lat1_rad
+        dlon = lon2_rad - lon1_rad
+        a = (
+            math.sin(dlat / 2) ** 2
+            + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(dlon / 2) ** 2
+        )
+        c = 2 * math.asin(math.sqrt(a))
+
+        earth_radius = 6371000  # meters
+        horizontal_distance = earth_radius * c
+
+        vertical_distance = abs(alt2 - alt1)
+
+        return math.sqrt(horizontal_distance**2 + vertical_distance**2)
+
+    def interpolate_position(
+        self, start: tuple[float, float, float], end: tuple[float, float, float], progress: float
+    ) -> tuple[float, float, float]:
+        """
+        Interpolate between two positions.
+
+        Args:
+            start: Starting position (lat, lon, alt)
+            end: Ending position (lat, lon, alt)
+            progress: Progress from 0.0 to 1.0
+
+        Returns:
+            Interpolated position (lat, lon, alt)
+        """
+        if progress >= 1.0:
+            return end
+        if progress <= 0.0:
+            return start
+
+        lat = start[0] + (end[0] - start[0]) * progress
+        lon = start[1] + (end[1] - start[1]) * progress
+        alt = start[2] + (end[2] - start[2]) * progress
+
+        return (lat, lon, alt)
+
     def periodic(self) -> None:
         """
-        Periodic function.
+        Periodic function that handles gradual movement to waypoints.
         """
+        current_time = time.time()
+        dt = current_time - self.last_update_time
+        self.last_update_time = current_time
+
+        self.get_target_position()
 
-        self.target_position = self.get_target_position()
+        # If we have a waypoint and we're not already there
+        if self.waypoint_position is not None:
+            distance_to_waypoint = self.calculate_distance(
+                self.current_position, self.waypoint_position
+            )
 
+            # If we're close enough to the waypoint, consider it reached
+            if distance_to_waypoint < 1.0:
+                print(
+                    f"HITL Position: Reached waypoint {self.waypoint_position[0]:.6f}, {self.waypoint_position[1]:.6f}, {self.waypoint_position[2]:.1f}m"
+                )
+                self.current_position = self.waypoint_position
+                self.target_position = self.waypoint_position
+                self.waypoint_position = None  # Clear waypoint
+            else:
+                # Move towards the waypoint
+                distance_to_move = self.movement_speed * dt
+                progress = min(distance_to_move / distance_to_waypoint, 1.0)
+
+                self.current_position = self.interpolate_position(
+                    self.current_position, self.waypoint_position, progress
+                )
+                self.target_position = self.current_position
+
+        # Inject the current interpolated position
         self.inject_position(
-            self.target_position[0], self.target_position[1], self.target_position[2]
+            self.current_position[0], self.current_position[1], self.current_position[2]
         )
 
+        time.sleep(0.1)
+
     def inject_position(
         self,
         latitude: float = 43.43405014107003,

modules/mavlink/flight_controller.py

@@ -211,11 +211,16 @@ def create(
         position_module: bool = False,
         camera_module: bool = False,
         images_path: str | None = None,
+        movement_speed: float = 5.0,
     ) -> "tuple[bool, FlightController | None]":
         """
         address: TCP address or serial port of the drone (e.g. "tcp:127.0.0.1:14550").
         baud: Baud rate for the connection (default is 57600).
-        mode: True to enable HITL mode, False or None to disable it.
+        hitl_enabled: True to enable HITL mode, False to disable it.
+        position_module: True to enable position emulation.
+        camera_module: True to enable camera emulation.
+        images_path: Path to images directory for camera emulation.
+        movement_speed: Speed of drone movement in m/s for position interpolation (default: 5.0).
         Establishes connection to drone through provided address
         and stores the DroneKit object.
         """
@@ -227,7 +232,7 @@ def create(
             )
             # Enable/disable HITL based on mode
             success, hitl_instance = hitl_base.HITL.create(
-                drone, hitl_enabled, position_module, camera_module, images_path
+                drone, hitl_enabled, position_module, camera_module, images_path, movement_speed
             )
             if success:
                 if hitl_enabled and hitl_instance is not None: