added multiprocessing worker and conversion methods

modules/drone_odometry_local.py

@@ -1,8 +1,12 @@
+"""
+Data structure for local odometry data (local position, orientation, and timestamp)
+"""
 
 import time
 
 from .common.mavlink.modules import drone_odometry
 
+
 class DronePositionLocal:
     """
     Drone position using NED.
@@ -12,27 +16,26 @@ class DronePositionLocal:
 
     @classmethod
     def create(
-            cls, 
-            north: float, 
-            east: float, 
-            down: float
-        ) -> "tuple[bool, DronePositionLocal | None]":
-        return True, DronePositionLocal(north, east, down)
-
-    def __init__(
-            self,
-            create_key: object, 
-            north: float, 
-            east: float, 
-            down: float
-        ) -> None:
-
+        cls, north: float, east: float, down: float
+    ) -> "tuple[bool, DronePositionLocal | None]":
+        """
+        Local position (NED)
+        """
+        return True, DronePositionLocal(cls.__create_key, north, east, down)
+
+    def __init__(self, create_key: object, north: float, east: float, down: float) -> None:
+
         assert create_key is DronePositionLocal.__create_key, "Use create() method"
 
         self.north = north
         self.east = east
         self.down = down
 
+    def __str__(self) -> str:
+        """
+        String representation
+        """
+        return f"{self.__class__}: North: {self.north}, East: {self.east}, Down: {self.down}."
 
 
 class DroneOdometryLocal:
@@ -45,29 +48,43 @@ class DroneOdometryLocal:
 
     @classmethod
     def create(
-            cls, 
-            drone_position: DronePositionLocal, 
-            drone_orientation: drone_odometry.DroneOrientation
-        ) -> "tuple[bool, DroneOdometryLocal | None]":
-
-        if drone_position is None or drone_orientation is None:
+        cls, local_position: DronePositionLocal, drone_orientation: drone_odometry.DroneOrientation
+    ) -> "tuple[bool, DroneOdometryLocal | None]":
+        """
+        Combines local odometry data with timestamp
+        """
+
+        if local_position is None:
             return False, None
-
+
+        if drone_orientation is None:
+            return False, None
+
         timestamp = time.time()
 
-        return True, DroneOdometryLocal(drone_position, drone_orientation, timestamp)
-
+        return True, DroneOdometryLocal(
+            cls.__create_key, local_position, drone_orientation, timestamp
+        )
 
     def __init__(
-            self, 
-            create_key: object,
-            drone_position: DronePositionLocal, 
-            drone_orientation: drone_odometry.DroneOrientation, 
-            timestamp: float
-            ) -> None:
-        
+        self,
+        create_key: object,
+        local_position: DronePositionLocal,
+        drone_orientation: drone_odometry.DroneOrientation,
+        timestamp: float,
+    ) -> None:
+
         assert create_key is DroneOdometryLocal.__create_key, "Use create() method"
 
-        self.drone_position = drone_position
+        self.local_position = local_position
         self.drone_orientation = drone_orientation
         self.timestamp = timestamp
+
+    def __str__(self) -> str:
+        """
+        String representation
+        """
+        return f"{self.__class__},\
+            {self.local_position}, \
+                DroneOrientation: Roll: {self.drone_orientation.roll}, Pitch: {self.drone_orientation.pitch}, Yaw: {self.drone_orientation.yaw}.\
+                    Time: {self.timestamp}."

modules/flight_interface/conversions.py

@@ -0,0 +1,48 @@
+import pymap3d as pymap
+
+from .. import drone_odometry_local
+from ..common.mavlink.modules import drone_odometry
+
+
+def position_global_to_local(
+    global_position, home_location
+) -> "tuple[bool, drone_odometry_local.DronePositionLocal | None]":
+    """
+    Converts global position (geodetic) to local position (NED)
+    """
+    north, east, down = pymap.geodetic2ned(
+        global_position.latitude,
+        global_position.longitude,
+        global_position.altitude,
+        home_location.latitude,
+        home_location.longitude,
+        home_location.altitude,
+    )
+
+    result, local_position = drone_odometry_local.DronePositionLocal.create(north, east, down)
+    if not result:
+        return False, None
+
+    return True, local_position
+
+
+def position_local_to_global(
+    local_position, home_location
+) -> "tuple[bool, drone_odometry.DronePosition | None]":
+    """
+    Converts local position (NED) to global position (geodetic)
+    """
+    latitude, longitude, altitude = pymap.ned2geodetic(
+        local_position.north,
+        local_position.east,
+        local_position.down,
+        home_location.latitude,
+        home_location.longitude,
+        home_location.altitude,
+    )
+
+    result, global_position = drone_odometry.DronePosition.create(latitude, longitude, altitude)
+    if not result:
+        return False, None
+
+    return True, global_position

modules/flight_interface/flight_interface.py

@@ -1,53 +1,58 @@
-
-from . import conversions # to be implemented
+from . import conversions
 from .. import drone_odometry_local
 
 from ..common.mavlink.modules import drone_odometry
 from ..common.mavlink.modules import flight_controller
 
+
 class FlightInterface:
+    """
+    Create flight controller and sets home location
+    """
 
     __create_key = object()
 
-    def create(
-        cls,
-        address: str,
-        timeout_home: float
-    ) -> "tuple[bool, FlightInterface | None]":
+    @classmethod
+    def create(cls, address: str, timeout_home: float) -> "tuple[bool, FlightInterface | None]":
+        """
+        address: TCP address or port.
+        timeout_home: Timeout for home location in seconds.
+        """
         result, controller = flight_controller.FlightController.create(address)
         if not result:
             return False, None
-        
+
         result, home_location = controller.get_home_location(timeout_home)
         if not result:
             return False, None
-        
+
         return True, FlightInterface(cls.__create_key, controller, home_location)
 
     def __init__(
-        self, 
+        self,
         create_key: object,
         controller: flight_controller.FlightController,
-        home_location: drone_odometry.DroneLocation, 
+        home_location: drone_odometry.DroneLocation,
     ) -> None:
         assert create_key is FlightInterface.__create_key, "Use create() method"
 
         self.controller = controller
         self.home_location = home_location
 
     def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]":
-
-        result, drone_odometry = self.controller.get_odometry()
+        """
+        Returns local drone odometry with timestamp
+        """
+        result, odometry = self.controller.get_odometry()
         if not result:
             return False, None
 
-        drone_position = drone_odometry.position
-        
-        result, drone_position_local = conversions.global_to_local(drone_position, self.home_location)
+        global_position = odometry.position
+
+        result, local_position = conversions.global_to_local(global_position, self.home_location)
         if not result:
             return False, None
 
-        drone_orientation = drone_odometry.orientation
+        drone_orientation = odometry.orientation
 
-        return drone_odometry_local.DroneOdometryLocal.create(drone_position_local, drone_orientation)
-
+        return drone_odometry_local.DroneOdometryLocal.create(local_position, drone_orientation)

modules/flight_interface/flight_interface_worker.py

@@ -0,0 +1,36 @@
+import time
+
+from . import flight_interface
+from ..worker import queue_wrapper
+from ..worker import worker_controller
+
+
+def flight_interface_worker(
+    address: str,
+    timeout: float,
+    period: float,
+    output_queue: queue_wrapper.QueueWrapper,
+    controller: worker_controller.WorkerController,
+) -> None:
+    """ 
+    Worker process.
+
+    address, timeout is initial setting.
+    period is minimum period between loops.
+    output_queue is the data queue.
+    controller is how the main process communicates to this worker process.
+    """
+    result, interface = flight_interface.FlightInterface.create(address, timeout)
+    if not result:
+        return
+
+    while not controller.is_exit_requested():
+        controller.check_pause()
+
+        time.sleep(period)
+
+        result, value = interface.run()
+        if not result:
+            continue
+
+        output_queue.queue.put(value)