rewrote flight interface to receive commands

UWARG · Jun 18, 2024 · f2e6d43 · f2e6d43
1 parent 3706402
commit f2e6d43
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 14 deletions.
diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py
@@ -3,10 +3,12 @@
 """
 
 from . import conversions
-from .. import drone_odometry_local
 
-from ..common.mavlink.modules import drone_odometry
-from ..common.mavlink.modules import flight_controller
+from modules import drone_odometry_local
+from modules import decision_command
+
+from common.mavlink.modules import drone_odometry
+from common.mavlink.modules import flight_controller
 
 
 class FlightInterface:
@@ -47,10 +49,13 @@ def __init__(
         self.controller = controller
         self.home_location = home_location
 
-    def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]":
+    def run(self, command: decision_command.DecisionCommand) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]":
         """
-        Returns local drone odometry with timestamp.
+        Uploads decision commands to drone and returns local drone odometry with timestamp.
         """
+        if command is not None:
+            self.controller.upload_commands(command.commands)
+
         result, odometry = self.controller.get_odometry()
         if not result:
             return False, None

diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py
@@ -3,18 +3,22 @@
 """
 
 import time
+import queue
 
 from worker import queue_wrapper
 from worker import worker_controller
 
+from modules import decision_command
+
 from . import flight_interface
 
 
 def flight_interface_worker(
     address: str,
     timeout: float,
     period: float,
-    output_queue: queue_wrapper.QueueWrapper,
+    command_in_queue: queue_wrapper.QueueWrapper,
+    odometry_out_queue: queue_wrapper.QueueWrapper,
     controller: worker_controller.WorkerController,
 ) -> None:
     """
@@ -25,6 +29,7 @@ def flight_interface_worker(
     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
@@ -34,8 +39,15 @@ def flight_interface_worker(
 
         time.sleep(period)
 
-        result, value = interface.run()
+        command = None
+
+        try:
+            command: decision_command.DecisionCommand = command_in_queue.queue.get()
+        except queue.Empty:
+            pass
+
+        result, value = interface.run(command)
         if not result:
             continue
 
-        output_queue.queue.put(value)
+        odometry_out_queue.queue.put(value)
diff --git a/tests/integration/test_flight_interface_worker.py b/tests/integration/test_flight_interface_worker.py
@@ -23,19 +23,20 @@ def main() -> int:
     """
     Main function.
     """
-
     controller = worker_controller.WorkerController()
-    manager = mp.Manager()
+    mp_manager = mp.Manager()
 
-    output_queue = queue_wrapper.QueueWrapper(manager, QUEUE_MAX_SIZE)
+    command_in_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE)
+    odometry_out_queue = queue_wrapper.QueueWrapper(mp_manager, QUEUE_MAX_SIZE)
 
     worker = mp.Process(
         target=flight_interface_worker.flight_interface_worker,
         args=(
             FLIGHT_INTERFACE_ADDRESS,
             FLIGHT_INTERFACE_TIMEOUT,
             FLIGHT_INTERFACE_WORKER_PERIOD,
-            output_queue,
+            command_in_queue,
+            odometry_out_queue,
             controller,
         ),
     )
@@ -46,7 +47,7 @@ def main() -> int:
 
     while True:
         try:
-            input_data: drone_odometry_local.DroneOdometryLocal = output_queue.queue.get_nowait()
+            input_data: drone_odometry_local.DroneOdometryLocal = odometry_out_queue.queue.get_nowait()
             assert (
                 str(type(input_data)) == "<class 'modules.drone_odometry_local.DroneOdometryLocal'>"
             )
@@ -67,6 +68,8 @@ def main() -> int:
 
     controller.request_exit()
 
+    command_in_queue.fill_and_drain_queue()
+
     worker.join()
 
     return 0

diff --git a/tests/unit/test_flight_interface.py b/tests/unit/test_flight_interface.py
@@ -7,7 +7,7 @@
 
 def create_flight_interface_instance(
     address: str, timeout: float
-) -> "tuple[bool, FlightInterface | None]":
+) -> "tuple[bool, flight_interface.FlightInterface | None]":
     """
     Construct a flight interface instance.
     """