created flight interface module

modules/flight_interface/conversions.py

@@ -0,0 +1,53 @@
+"""
+Drone position conversions to and from local (NED) and global (geodetic) space.
+"""
+
+import pymap3d as pymap
+
+from .. import drone_odometry_local
+from ..common.mavlink.modules import drone_odometry
+
+
+def position_global_to_local(
+    global_position: drone_odometry.DronePosition, home_location: drone_odometry.DronePosition
+) -> "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: drone_odometry_local.DronePositionLocal,
+    home_location: drone_odometry.DronePosition,
+) -> "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

@@ -0,0 +1,66 @@
+"""
+Creates flight controller and produces local drone odometry coupled with a timestamp.
+"""
+
+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()
+
+    @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,
+        create_key: object,
+        controller: flight_controller.FlightController,
+        home_location: drone_odometry.DroneLocation,
+    ) -> None:
+        """
+        Private constructor, use create() method.
+        """
+
+        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]":
+        """
+        Returns local drone odometry with timestamp.
+        """
+        result, odometry = self.controller.get_odometry()
+        if not result:
+            return False, None
+
+        global_position = odometry.position
+
+        result, local_position = conversions.global_to_local(global_position, self.home_location)
+        if not result:
+            return False, None
+
+        drone_orientation = odometry.orientation
+
+        return drone_odometry_local.DroneOdometryLocal.create(local_position, drone_orientation)

modules/flight_interface/flight_interface_worker.py

@@ -0,0 +1,40 @@
+"""
+Fetches local drone odometry.
+"""
+
+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)

tests/integration/test_flight_interface_worker.py

@@ -0,0 +1,70 @@
+"""
+Flight interface worker integration test.
+"""
+
+import multiprocessing as mp
+import queue
+import time
+
+from worker import worker_controller
+from worker import queue_wrapper
+
+from modules import drone_odometry_local
+from modules.flight_interface import flight_interface_worker
+
+QUEUE_MAX_SIZE = 10
+FLIGHT_INTERFACE_ADDRESS = "tcp:127.0.0.1:14550"
+FLIGHT_INTERFACE_TIMEOUT = 10
+FLIGHT_INTERFACE_WORKER_PERIOD = 0.1
+
+
+def main() -> int:
+    """
+    Main function.
+    """
+
+    controller = worker_controller.WorkerController()
+    manager = mp.Manager()
+
+    output_queue = queue_wrapper.QueueWrapper(manager, QUEUE_MAX_SIZE)
+
+    args = (
+        FLIGHT_INTERFACE_ADDRESS,
+        FLIGHT_INTERFACE_TIMEOUT,
+        FLIGHT_INTERFACE_WORKER_PERIOD,
+        output_queue,
+        controller,
+    )
+
+    worker = mp.Process(target=flight_interface_worker, args=args)
+
+    worker.start()
+
+    time.sleep(3)
+
+    while True:
+        try:
+            input_data: drone_odometry_local.DroneOdometryLocal = output_queue.queue.get()
+            assert (
+                str(type(input_data))
+                == "<class 'modules.drone_odometry_local\
+                    .DroneOdometryLocal'>"
+            )
+            assert input_data.local_position is not None
+            assert input_data.drone_orientation is not None
+        except queue.Empty:
+            break
+
+    controller.request_exit()
+
+    worker.join()
+
+    return 0
+
+
+if __name__ == "__main__":
+    result_main = main()
+    if result_main < 0:
+        print("error: " + result_main)
+
+    print("Done.")

tests/unit/test_flight_interface.py

@@ -0,0 +1,92 @@
+"""
+Test for flight interface by printing to device.
+"""
+
+import time
+import pytest
+
+
+from modules.flight_interface import flight_interface
+
+from ..common.mavlink.modules import drone_odometry
+
+
+@pytest.fixture()
+def create_flight_interface_instance(
+    address: str, timeout: float
+) -> "tuple[bool, FlightInterface | None]":
+    """
+    Construct a flight interface instance.
+    """
+    result, flight_interface_instance = flight_interface.FlightInterface.create(address, timeout)
+
+    yield result, flight_interface_instance
+
+
+@pytest.fixture()
+def create_drone_position(
+    latitude: float, longitude: float, altitude: float
+) -> drone_odometry.DronePosition:
+    """
+    Contruct a drone position instance.
+    """
+    result, global_position = drone_odometry.DronePosition.create(latitude, longitude, altitude)
+    assert result
+    assert global_position is not None
+
+    yield global_position
+
+
+class TestFlightInterface:
+    """
+    Flight interface tests.
+    """
+
+    MISSION_PLANNER_ADDRESS = "tcp:127.0.0.1:14550"
+    TIMEOUT = 1.0
+
+    DELAY_TIME = 1.0
+
+    def test_create_invalid_address(self) -> None:
+        """
+        Test create method using a valid Mission Planner IP address.
+        """
+        result, instance = create_flight_interface_instance(None, self.TIMEOUT)
+        assert not result
+        assert instance is None
+
+    def test_create_valid_address(self) -> None:
+        """
+        Test create method using a valid Mission Planner IP address.
+        """
+        result, instance = create_flight_interface_instance(
+            self.MISSION_PLANNER_ADDRESS, self.TIMEOUT
+        )
+        assert result
+        assert instance.controller is not None
+        assert instance.home_location is not None
+
+    def test_flight_interface(self) -> None:
+        """
+        Tests run function and prints results.
+        """
+
+        result, flight_interface_instance = create_flight_interface_instance(
+            self.MISSION_PLANNER_ADDRESS, self.TIMEOUT
+        )
+
+        for _ in range(8):
+            result, local_odometry = flight_interface_instance.run()
+            assert result
+            assert local_odometry is not None
+
+            print("north: " + str(local_odometry.local_position.north))
+            print("east: " + str(local_odometry.local_position.east))
+            print("down: " + str(local_odometry.local_position.down))
+            print("roll: " + str(local_odometry.drone_orientation.roll))
+            print("pitch: " + str(local_odometry.drone_orientation.pitch))
+            print("yaw: " + str(local_odometry.drone_orientation.yaw))
+            print("timestamp: " + str(local_odometry.timestamp))
+            print("")
+
+            time.sleep(self.DELAY_TIME)